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The  Joint  Architecture  for  Unmanned  Systems  (JAUS)  has  successfully  established 
a  well-defined  component  interface  for  unmanned  mobile  systems,  but  has  yet  to  address 
the  implications  of  such  systems  requiring  an  on-board  robot  manipulator.  This 
configuration  is  seen  in  many  applications  including  planetary  exploration,  hazardous 
materials  removal,  and  marine  research  and  is  frequently  referred  to  as  the  vehicle- 
manipulator  system.  The  purpose  of  our  study  was  to  develop  and  implement  a  set  of 
JAUS  components  that  will  allow  for  tele-operational  (teleop)  and  autonomous  control  of 
a  vehicle-manipulator  platform.  Teleop  control  is  the  control  of  a  system  by  the  direct 
input  of  a  human  or  a  computer.  Autonomous  control  is  a  cooperative  mode  between  the 
vehicle  and  the  manipulator. 

Testing  and  implementation  of  these  components  was  performed  on  a  6-degree-of- 
freedom  (6-DOF)  Puma  762  robot  manipulator  (Unimation  -  Westinghouse,  Danbury, 
Connecticut)  outfitted  with  a  commercially  available  Galil  motion  controller  (Galil 
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Motion  Control,  Inc,  Rocklin,  California).  Successful  completion  and  adequate 
compatibility  to  the  outlined  JAUS  perfonnance  specifications  would  ensure  the 
inception  of  the  above  mentioned  components  to  the  latest  version  of  the  document’s 
Reference  Architecture.  Even  though  this  modular  manipulator  architecture  is  suitable  for 
both  autonomous  and  teleop  control,  testing  and  results  were  based  solely  on  the  input 
provided  using  a  graphical  user  interface  on  a  computer. 


CHAPTER  1 
INTRODUCTION 

1.1  Introduction 

In  the  past  decade,  significant  technological  breakthroughs  have  led  to  smarter  and 
more-reliable  unmanned  systems.  Reliability  and  robustness  of  these  systems  depend  on 
many  factors,  most  of  which  are  defined  in  the  early  stages  of  design  and  development. 
With  an  increasing  interest  in  this  field  and  tremendous  implications  to  both  civilian  and 
military  use,  there  is  a  growing  demand  for  a  set  of  well-defined  standards  that  could 
ensure  safety,  reliability,  and  interoperability.  The  Center  for  Intelligent  Machines  and 
Robotics  (CIMAR)  at  the  University  of  Florida  has  been  involved  in  developing  of  one 
such  standard,  in  cooperation  with  the  Department  of  Defense. 

The  Joint  Architecture  for  Unmanned  Systems  (JAUS)  has  successfully  established 
a  well-defined  component  interface  for  unmanned  mobile  systems,  but  has  yet  to  address 
the  implications  of  such  systems  requiring  an  on-board  robot  manipulator.  This 
configuration  is  seen  in  many  applications  including  planetary  exploration,  hazardous 
materials  removal,  and  marine  research;  and  is  frequently  referred  to  as  a 
vehicle-manipulator  system.  The  purpose  of  our  study  is  to  develop  and  implement  a  set 
of  JAUS  components  that  will  allow  for  tele-operational  (teleop)  and  autonomous  control 
of  a  vehicle-manipulator  platform.  Teleop  control  is  defined  as  the  control  of  a  system  by 
the  direct  input  of  a  human  or  a  computer.  Autonomous  control  is  defined  as  a 
cooperative  mode  between  the  vehicle  and  the  manipulator.  The  components  focus  on  a 
serial  manipulator  comprising  of  any  number  of  prismatic  and  revolute  joints.  Parallel 
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mechanisms  are  not  addressed  as  a  part  of  our  study.  The  components  are  grouped 

according  to  function  into  the  following  categories: 

•  Low-level  manipulator  control  components:  The  one  component  in  this  category 
allows  for  low-level  command  of  the  manipulator  joint-actuation  efforts.  This  is  an 
open-loop  command  that  could  be  used  in  a  simple  tele-operation  scenario.  The 
component  in  this  category  is  listed  as  follows: 

o  Primitive  manipulator  component 

•  Manipulator  sensor  components:  These  components,  when  queried,  return 
instantaneous  sensor  data.  Three  components  are  defined  that  return  respectively 
joint  positions,  joint  velocities,  and  joint  torques  or  forces.  The  components  in  this 
category  are  listed  as  follows: 

o  Manipulator  joint  position  sensor  component 

o  Manipulator  joint  velocity  sensor  component 

o  Manipulator  joint  force/torque  sensor  component 

•  Low-level  position  and  velocity  driver  components:  These  components  take  as 
inputs  the  desired  joint  positions,  the  desired  joint  velocities,  the  desired 
end-effector  pose,  or  the  desired  end-effector  velocity  state.  Closed-loop  control  is 
implied.  No  path  information  is  specified.  The  components  in  this  category  are 
listed  as  follows: 

o  Manipulator  joint  positions  driver  component 

o  Manipulator  end-effector  pose  driver  component 
o  Manipulator  joint  velocities  driver  component 

o  Manipulator  end-effector  velocity  state  driver  component 

•  Mid-level  position  and  velocity  driver  components:  Two  components  are 
grouped  under  this  heading.  The  first  takes  as  input  the  goal  values  for  each  joint 
parameter  at  several  time  values  together  with  motion  constraints  (i.e.  maximum 
joint  velocity,  maximum  acceleration,  and  maximum  deceleration).  The  second 
takes  as  input  a  series  of  end-effector  poses  at  specified  time  values.  Closed-loop 
control  is  implied.  The  components  in  this  category  are: 

o  Manipulator  Joint  Move  Driver  Component 

o  Manipulator  End-Effector  Discrete  Pose  Driver  Component. 
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Testing  and  implementation  of  these  components  is  performed  on  a 
6-degree-of-freedom  (6-DOF)  Puma  762  robot  manipulator  outfitted  with  a  commercially 
available  Galil  motion  controller.  Successful  completion  and  adequate  compatibility  to 
the  outlined  JAUS  performance  specifications  would  ensure  the  inception  of  the 
above-mentioned  components  to  the  latest  version  of  the  document’s  Reference 
Architecture.  Even  though  this  modular-manipulator  architecture  is  suitable  for  both 
autonomous  and  teleop  control,  testing  and  results  are  based  solely  on  input  provided 
using  a  graphical  user  interface  on  a  computer. 

1.2  Background 

Robot  manipulators  are  used  in  a  wide  variety  of  applications;  but  usually  follow 
user-defined  paths,  thus  requiring  teleop  control.  In  the  case  of  a  manipulator  aboard  an 
autonomous  mobile  system,  there  is  a  growing  demand  for  the  manipulator  and  the 
vehicle  to  be  able  to  cooperate,  allowing  the  system  to  autonomously  complete 
more-complex  tasks  (such  as  sample  acquisition,  instrument  placement,  and  mobility 
assistance).  This  problem  was  first  addressed  by  NASA  in  the  mid  1990s  as  the  first  rover 
missions  to  Mars  were  planned.  Technology  developed  in  1998  [1]  allowed  robot 
manipulators  aboard  the  vehicles  to  autonomously  acquire  small  rock  samples 
(designated  by  the  user)  within  1  meter  away;  and  to  place  instruments  on  targets  less 
than  5  meters  away.  These  capabilities  are  accomplished  with  onboard  vision  sensors 
using  stereo  processing  algorithms  developed  by  Matthies  [2].  This  application  showed 
the  autonomous  behavior  of  the  manipulator  but  did  not  consider  other  high-level  vehicle 
functions.  The  algorithms  were  designed  to  be  portable,  extendible,  and  reusable  across 
many  vehicle  platfonns.  Testing  was  successfully  completed  on  Rocky  7  [3]. 
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Other  inhospitable  environments  (such  as  oceans)  are  frequently  explored  using 
underwater  vehicle-manipulator  systems  (UVMS).  These  systems  are  mostly  used  for 
inspection,  drilling,  mine  countermeasures,  and  surveying  [4].  In  most  applications, 
UVMS  operates  as  a  master-slave  configuration,  and  as  such  is  prone  to  a  few 
deficiencies.  Challenges  include  correctly  modeling  the  nonlinear  hydrodynamic  effects 
of  the  environment;  and  improving  the  performance  of  motion  control  encompassing 
singularity  avoidance,  obstacle  detection,  and  power  optimization  [5],  Recent  research  [6] 
in  the  field  of  UVMS  uses  a  unified  force  control  approach,  which  combines  impedance 
control  with  hybrid  position/force  control  by  means  of  fuzzy  switching  to  perform 
autonomous  underwater  manipulation. 

1.3  Joint  Architecture  for  Unmanned  Systems  (JAUS) 

1.3.1  Overview 

The  Joint  Architecture  for  Unmanned  Systems  (JAUS)  [7]  is  being  developed  in 
conjunction  with  the  Department  of  Defense  and  many  other  members  of  industry  and 
academia  for  use  in  research,  development  and  acquisition  of  unmanned  systems.  The 
current  version  of  the  document  is  divided  into  three  large  volumes;  the  JAUS  Domain 
Model  (Vol.  I),  JAUS  Reference  Architecture  (RA-Vol.  II),  and  JAUS  Document  Control 
Plan  (Vol.  III).  The  JAUS  Working  Group  was  chartered  to  reduce  lifecycle  costs,  and 
integration  and  development  time;  to  provide  a  framework  for  technology  insertion;  to 
accommodate  expansion  of  existing  systems  with  new  capabilities. 

Volume  I  defines  known  and  prospective  operational  requirements  of  unmanned 
systems.  Volume  III  defines  the  process  used  to  identify  and  track  requested  changes  to 
accepted  JAUS  documentation.  Components  developed  as  a  part  of  our  study  affect  all 
three  parts  of  Volume  II  because  it  is  concerned  with  aspects  of  component  design.  The 
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main  purpose  of  the  Reference  Architecture  (RA)  is  to  describe  all  functions  and 
messages  that  shall  be  used  to  design  new  components.  Joint  Architecture  for  Unmanned 
Systems  defines  components  for  all  classifications  of  Unmanned  Systems,  from  teleop  to 
autonomous.  As  a  particular  system  evolves,  the  architecture  is  already  in  place  to 
support  more-advanced  capabilities.  To  meet  this  requirement,  four  technical  constraints 
are  imposed  on  JAUS: 

•  Platform  independence:  Since  unmanned  systems  will  be  based  on  a  variety  of 
missions,  no  assumptions  are  made  regarding  the  vehicle  platform. 

•  Mission  isolation:  Joint  Architecture  for  Unmanned  Systems  defines  a  mission  as 
the  ability  to  gather  information  about  or  to  alter  the  state  of  the  environment  in 
which  the  platfonn  is  operating.  This  allows  the  developer  to  construct  the  system 
to  support  a  variety  of  missions. 

•  Computer  hardware  independence:  Advances  in  computer  technology  over  the 
past  couple  of  decades  have  seen  rapid  growth.  The  JAUS  computer  hardware 
constraint  was  put  in  place  to  ensure  software  and  hardware  portability  as  new 
systems  are  developed  in  the  future. 

•  Technology  independence:  This  constraint  is  similar  to  hardware  independence, 
but  focuses  more  on  technical  approach  [7,  8],  In  this  particular  application,  the 
architecture  makes  no  assumption  regarding  the  method  used  to  obtain  joint 
positions.  For  example  different  manipulators  could  be  outfitted  by  different 
position  sensors  that  include  encoders,  potentiometers,  or  rotational  variable 
differential  transformers. 

1.3.2  Standards 

The  Reference  Architecture  (RA)  document  defines  the  JAUS  specific  protocol  for 
transmission  of  messages.  Aside  from  computer  code  specifications,  this  section  also 
standardizes  mathematical  notations.  The  notation  used  in  this  thesis  is  standard  to  many 
popular  robotics  textbooks;  specifically,  we  use  the  definitions  covered  by  Crane  and 
Duffy  [9].  To  standardize  the  method  of  referencing  manipulator  links  (or  joints),  Figures 
1-1  through  1-3  illustrate  the  notation  that  will  be  used  later  in  the  document.  Figure  1-1 
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shows  the  parameters  used  for  link  ij.  Figure  1-2  and  Figure  1-3  show  additional 
parameters  used  to  define  rotational  joints  and  prismatic  joints,  respectively. 


Figure  1-1.  Manipulator  linkage  parameters  for  link  ij  (Source:  Crane  C.,  Duffy  J., 
Kinematic  Analysis  of  Robot  Manipulators,  Cambridge  University  Press, 
1998,  p.  21,  Figure  3.2) 

Link  length  ay  is  measured  as  the  perpendicular  distance  between  joint  axis  i  and 
joint  axis  j  along  the  unit  vector  ay.  Note  that  it  can  have  a  negative  value.  Twist  angle 
ay  is  the  angle  between  S,  (unit  vector  along  joint  axis  i)  and  Sj  (unit  vector  along  joint 
axis  j)  measured  in  a  right  hand  sense  about  ay. 


Figure  1-2.  Manipulator  linkage  parameters  for  revolute  joint  j  (Source:  Crane  C.,  Duffy 
J.,  Kinematic  Analysis  of  Robot  Manipulators,  Cambridge  University  Press, 
1998,  p.  22,  Figure  3.5) 
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Constant  joint  offset  Sj  is  measured  as  the  perpendicular  distance  between  link  a,, 
and  Link  along  the  unit  vector  Sj.  Note  that  it  can  have  a  negative  value.  Variable 
joint  angle  9j  is  the  angle  between  a,j  (unit  vector  along  link  ij)  and  ajk  (unit  vector  along 
link  jk)  measured  in  a  right  hand  sense  about  Sj. 

Sj 


Figure  1-3.  Manipulator  linkage  parameters  for  prismatic  joint  j  (Source:  Crane  C.,  Duffy 
J.,  Kinematic  Analysis  of  Robot  Manipulators,  Cambridge  University  Press, 
1998,  p.  22,  Figure  3.6) 

Variable  joint  offset  Sj  is  measured  as  the  perpendicular  distance  between  link  ajj 
and  link  ajk  along  the  unit  vector  Sj.  Note  that  it  can  have  a  negative  value.  Joint  angle  0, 
is  the  angle  between  a,,  (unit  vector  along  link  ij)  and  ajk  (unit  vector  along  link  jk) 
measured  in  a  right  hand  sense  about  Sj. 

1.3.3  System  Topology 

The  Joint  Architecture  for  Unmanned  Systems  hierarchy  [7]  is  comprised  of  four 
elements:  system,  subsystem,  node,  and  component/instance.  Figures  1-4  and  1-5  show 
the  interaction  of  the  four  hierarchical  elements  in  general  and  with  respect  to  the 
manipulator  extension.  A  system  is  defined  as  a  logical  grouping  of  one  or  more 
subsystems  allowing  for  cooperative  advantage  between  the  constituents.  A  subsystem  is 
an  independent  unit  consisting  of  a  number  of  nodes  supporting  its  functional 
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requirements  and  defining  an  operational  unmanned  system.  The  scope  of  our  study  will 
encompass  the  use  of  all  but  the  system  element,  thus  in  this  particular  implementation, 
subsystem  would  correspond  to  the  vehicle  hosting  the  manipulator. 


Figure  1-4.  Architecture  hierarchy 


SYSTEM 

PUMA-20 

|  SARCOS-21 

Manpulaftir-Tl 

Node 

Node 

Node 

Pnmtovc  Manpula?or-49 


r 

Manpiiakx  Jorrt  Position  Sensor- 51 

MancnJatcf  Jont  Position  Sensor- 52 

1 1  Manipubtor  FarccTorque  Posnan  Sensor-53 

,  UAMF\IIATnRSrNSnRCnM»R««’NTS 

Manipulator  Joint  Positions  Dover -54 


Manipulator  Erd-Eltectror  Pose  Orrver-55 


Manipulator  Joint  Veiocr.ies  Dnver-56 


Manipulator  Erd-Etfectror  Velocity  Driver  State  Ortver-57 


LCHV  LfcVfc.  <*09110*  AKU  tfeLOCHY  JHIVfcK  LUWI'UNtM  3 


Manipulator  Jomt  Move  Ortver-58 


Manfiuator  End-€  tie  error  Discrete  Pose  Orrver  Dnver-59 


CompN  Instl 


Figure  1-5.  Architecture  hierarchy  of  the  manipulator  control  node 
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A  node  is  defined  as  a  distinct  entity,  composed  of  all  the  hardware  and  software 
necessary  to  support  a  well-defined  computing  capability.  In  the  case  of  a  vehicle- 
manipulator  system,  the  manipulator  control  node  contains  all  of  the  manipulator 
components  whether  they  are  a  part  of  a  single  or  multiple  processors  working  together. 
Finally,  a  component  or  an  instance  is  a  cohesive  software  process  [8].  An  instance  is  a 
single  occurrence  running  on  a  single  node. 

1.3.4  Component  Definition 

As  it  can  be  seen  in  Figure  1-5,  JAUS  is  a  hierarchical  system  of  components  with 
standardized  interfaces.  Each  component  has  a  distinct  name  and  identification  number 
and  performs  a  single,  cohesive  function.  Each  component  has  to  accept  the  core  JAUS 
message  set  as  well  as  the  input  and  output  messages  specific  to  the  component  itself. 
The  JAUS  core  message  set  [7]  is  shown  in  Table  1-1. 


Table  1-1.  The  JAUS  core  message  set 


Code 

Description 

0x01 

Set  component  authority 

0x02 

Shutdown 

0x03 

Standby 

0x04 

Resume 

0x05 

Reset 

0x06 

Set  emergency 

0x07 

Clear  emergency 

0x08 

Create  service  connection 

0x09 

Confirm  service  connection 

OxOA 

Activate  service  connection 

OxOB 

Suspend  service  connection 

OxOC 

Terminate  service  connection 

OxOD 

Request  component  control 

OxOE 

Release  component  control 

OxOF 

Confirm  component  control 

0x10 

Reject  component  control 
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1.3.5  Message  Specification 

JAUS  can  be  defined  as  component  based  message  passing  architecture.  As  such,  it 
uses  a  well  defined  set  of  messages  that  commence  actions,  exchange  infonnation,  and 
cause  events  to  occur.  JAUS  defines  six  classes  of  messages  at  the  component  level,  each 
of  which  will  be  used  in  the  manipulator  component  implementation.  Table  1-2  lists  the 
message  classes  [7], 


Table  1-2.  Segmentation  of  command  codes  by  class 


Message  Class 

Offset  Range  (OOOOh  to  FFFFh) 

Command 

OOOOh  -  IFFFh 

Query 

2000h  -  3FFFh 

Inform 

4000h  -  5FFFh 

Event  Setup 

6000h  -  7FFFh 

Event  Notification 

8000h  -  9FFFh 

Node  Management 

AOOOh  -  BFFFh 

Reserved 

COOOh  -  CFFFh 

User  Defined  Message 

DOOOh  -  FFFFh 

All  messages  are  composed  of  a  message  header  and  the  message  data  buffer.  The 
header  defines  the  message’s  destination  node,  component,  instance,  subsystem 
identification  and  the  message’s  corresponding  source  information.  The  header  also 
contains  the  JAUS  command  code,  the  number  of  bytes  in  the  data  buffer  that  the 
destination  component  can  expect  to  receive,  as  well  as  the  information  pertaining  to  the 
message  properties  [8],  The  header  fonnat  is  common  to  all  messages  as  shown  in  Table 
1-3,  allowing  JAUS  to  employ  an  embedded  protocol  providing  specific  information  on 
how  to  handle  encoding  before  and  after  the  transmission.  The  message  header,  at  a 
minimum,  should  be  included  in  all  messages.  Figure  1-6  and  Figure  1-7  show  message 
header  detail,  header  data  format  and  header  bit  layout,  respectively. 
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Figure  1-6.  The  JAUS  message  header  detail  (Source:  JAUS  Working  Group,  2004,  Joint 
Architecture  for  Unmanned  Systems  (JAUS):  Reference  Architecture 
Specification,  Version  3.2,  Volume  II,  The  Joint  Architecture  for  Unmanned 
Systems,  http://www.jauswg.org,  October  2004,  Part  2,  p.l  1,  Figure  3.1) 


*3 

to 


< 

to 

a. 


50 

to 


< 

to 

a. 


Version 
Range  0 ..  63 

Version  2.0  =  0 

Version  3.0  or  3.1  =  1 

Version  3.2  =  2 

3  ..  63  unused 


i 

w  2 

*  TO 

•■a  <*> 

ft  £ 

g  ft 
ft  hrt 

a  w 

&  CTQ 


I 

Xfl  gp 
ft  2 

2. 

TO  S 

O  Q 
o  2 

3  3 
3  2 
re  2 
re  2. 

a. 

o’  2 

o  2 

i  3 
O 


I  I 

3  3 
o  3 

-5  -S 

ft  ft 
<*>  {*} 

T3  TS 
O  O 
3  3 

O  ft 


2 

o 

3 

ft 


i  » 

1  o 

50  £ 

rt  a 

g 

ft 


3 

O 

£ 

3 

3 

7T 


Message  Priority 
Range  0  ..15 

Default  Priority  6 

Normal  Priority  Range 
0..  11 

Safety  Critical  Range 
12  ..  15 


15  14  13  12  11  10  09  08  07  06  05  04  03  02  01  00 


Figure  1-7.  Bit  layout  of  message  properties  (Source:  JAUS  Working  Group,  2004,  Joint 
Architecture  for  Unmanned  Systems  (JAUS):  Reference  Architecture 
Specification,  Version  3.2,  Volume  II,  The  Joint  Architecture  for  Unmanned 
Systems,  http://www.jauswg.org,  October  2004,  Part  2,  p.  1 3,  Figure  3.2) 


The  message  data  buffer  contains  packed  JAUS  control  data,  and  each  command 


code  has  control  data  used  by  the  system  to  command  component  behavior.  If  a  particular 
system  or  subsystem  contains  multiple  components  passing  multiple  messages 
simultaneously,  it  can  potentially  result  in  system  delays  due  to  bandwidth  overload.  In 
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order  to  prevent  this,  each  component  and  its  subsequent  command  codes  must  have  the 
ability  to  pack  and  unpack  (compress)  the  control  data  [8], 

Each  JAUS  component  and  message  is  well  constrained  as  described  in  the  sections 
above.  The  next  four  chapters  strive  to  provide  adequate  definitions  of  manipulator 
control  components  relative  to  the  JAUS  framework.  Chapter  8  covers  the  aspects  of 
software  design  and  details  of  the  component  and  message  implementation  onto  the 
PumaA  762  robot.  Finally,  Chapter  9  provides  the  results  in  terms  of  the  compatibility  of 
the  designed  JAUS  components  and  messages  with  the  constraints  outlined  by  JAUS.  It 
also  quantifies  the  perfonnance  specifications  of  the  system  using  the  new  architecture. 


Table  1-3.  Message  header  data  format 


Field  # 

Field  Description 

Type 

Size  (Bytes) 

1 

Message  Properties 

Unsigned  Short 

2 

2 

Command  Code 

Unsigned  Short 

2 

3 

Destination  Instance  ID 

Byte 

1 

4 

Destination  Component  ID 

Byte 

1 

5 

Destination  Node  ID 

Byte 

1 

6 

Destination  Subsystem  ID 

Byte 

1 

7 

Source  Instance  ID 

Byte 

1 

8 

Source  Component  ID 

Byte 

1 

9 

Source  Node  ID 

Byte 

1 

10 

Source  Subsystem  ID 

Byte 

1 

11 

Data  Control  (bytes) 

Unsigned  Short 

2 

12 

Sequence  Number 

Unsigned  Short 

2 

Total  Bytes 

16 

CHAPTER  2 

SYSTEM  ANALYSIS  AND  OVERVIEW 


Manipulator  JAUS  components  and  messages  are  designed  to  work  on  any  serial 
manipulator  regardless  of  the  type  or  the  number  of  joints.  As  a  result,  the  choice  of  the 
test  platform  for  implementation  purposes  is  irrelevant.  Due  to  the  availability  and  its 
operational  condition,  a  6-DOF  Puma  762  (Figure  2-1)  robot  ann  was  chosen  for  this 
task.  Even  though  it  was  originally  designed  for  industrial  purposes,  this  particular 
manipulator  can  be  found  in  many  research  laboratories  due  to  its  robustness  and 
reliability. 


Figure  2-1.  Puma  762,  6-DOF  robot  manipulator 

The  purpose  of  this  chapter  is  to  summarize  the  useful  mechanical  properties  and 
describe  the  mathematics  behind  position  and  velocity  analysis  of  the  Puma  762  robot. 
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The  theoretical  approach  and  notations  used  in  the  following  sections  specifically  follow 
definitions  outlined  by  Crane  and  Duffy  [9,  10]. 

2.1  Mechanism  Overview 

The  Puma  arm  can  be  compared  to  a  human  torso,  shoulder,  and  wrist.  It  consists 
of  members  connected  by  six  revolute  joints,  each  defining  an  axis  about  which  the 
members  rotate.  The  major  joints  are  equipped  with  limit-switch-shutoff-systems 
positioned  2  degrees  past  the  software  stops  providing  additional  safety.  The  Puma  762 
weighs  590  kg  and  has  a  maximum  static  payload  of  20  kg.  Table  2-1  lists  useful 
mechanical  specifications  of  the  system. 

Table  2-1.  Mechanical  specifications  of  the  Puma  762  robot _ 


Joint  # 

1 

2 

3 

4 

5 

6 

Software  Movement  Limits 

320 

220 

270 

532 

220 

532 

(deg) 

Joint  Angular  Resolution 
(deg  *  10'3) 

Encoder  Index  Resolution; 

5.0 

3.5 

4.5 

12.5 

6.2 

13.4 

equal  to  one  motor 
revolution  (deg) 

4.0 

2.78 

3.57 

6.26 

6.26 

13.4 

2.2  Kinematic  Analysis  of  the  Puma  762  Robot 

2.2.1  Notation 


Figures  1-2  and  1-3  show  detailed  definitions  of  vectors  and  parameters  used  to 
label  revolute  and  prismatic  joints,  respectively.  The  mechanism  parameters  listed  in 
Table  2-2  are  used  to  create  a  kinematic  chain  shown  in  Figure  2-2.  The  proper  analysis 
of  a  robot  manipulator  mandates  that  a  coordinate  system  is  attached  to  each  of  the 
bodies.  The  coordinate  system  attached  to  link  ij  is  called  the  ith  coordinate  system  [9].  Its 
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origin  is  located  at  the  intersection  of  vectors  St  and  aij ;  x-direction  is  along  the  vector 
Qy  ,  and  z-direction  is  along  Si . 


Table  2-2.  Mechanism  parameters  of  the  Puma  762  robot 


Link  Length,  mm 

Twist  Angle,  deg 

Jnt  Offset,  nnn 

Joint  Angle,  deg 
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Figure  2-2.  Labeled  kinematic  model  of  the  Puma  762  manipulator 

The  transfonnation  matrix  relating  two  of  these  coordinate  systems  in  a  three 
dimensional  space  is  given  by  Equation  2-1. 


0 

~sj 

0 

a 

V 

sjCij 

cjch 

~s« 

SjSij 

CjSj 

ca 

0 

0 

0 

1 

(2-1) 
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The  inverse  of  this  transformation  is  very  frequently  used  and  is  given  by  the 
following  4x4  transformation: 


cv 

sjCiJ 

SjSiJ 

C  jQ  ij 

~SJ 

cjcij 

cjsu 

SjaiJ 

0 

~sy 

cv 

-Sj 

0 

0 

0 

1 

(2-2) 


d  .  •  •  s  c 

where  ij  represents  a  link  length  of  a  serial  manipulator,  '  and  '  represent  the  sine 

and  cosine  of  ,  respectively.  Furthermore,  S'j  and  °iJ  represent  the  sine  and  cosine  ofa‘j . 
Finally,  a  fixed  coordinate  system  is  defined  as  having  its  origin  coincident  with  the 


C 

origin  of  the  first  coordinate  system  and  its  axis  along  the  vector  1 .  The  transformation 
that  relates  the  first  coordinate  system  and  the  fixed  is  given  in  Equation  2-3. 


cos^, 

sin^ 

0 

0 


-  sin  0  0 

cos  0  0 

0  1  0 

0  0  1 


(2-3) 


General  expressions  determining  the  directions  of  joint  vectors  with  respect  to  the 
first  coordinate  system  are  given  in  Equations  2-4  and  2-5. 
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(2-5) 

where  the  definitions  of  the  X,  Y,  Z  and  W,  U*,  U  are  presented  in  Appendix  A.  Using 
the  coordinate  transfonnation  given  in  Equation  2-3  and  expressions  from  Equations  2-4 
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and  2-5,  direction  vectors  can  be  obtained  in  terms  of  the  fixed  coordinate  system  and  are 
shown  in  Equations  2-6  and  2-7. 

FSi=FT1-1Si  (2-6) 


7  — *■  F  rp  1  — ► 

a  =  Tr  a 


(2-7) 


2.2.2  Forward  and  Reverse  Position  Analysis 

The  most  basic  problem  in  the  analysis  of  serial  manipulators  is  to  determine  the 
position  and  orientation  of  the  end-effector  for  a  specified  set  of  joint  angles.  Following 
the  method  outlined  by  Crane  and  Duffy  [9],  the  first  step  to  the  solution  determines  the 
transfonnation  that  relates  the  end-effector  coordinate  system  to  the  fixed  coordinate 
system.  In  the  case  of  a  6-axis  robot,  the  transformation  in  question  is  obtained  as 
follows: 


f6T=\T-\T-\T-\T-\T-\T 


(2-8) 


The  coordinates  of  the  end-effector  in  the  fixed  coordinate  system  can  be  found 
from  Equation  2-9. 


7  P  —F  T-6  P 

rtool  61  r tool 


(2-9) 


All  of  the  terms  in  Equation  2-8,  are  obtained  from  known  mechanism  parameters  that 
are  listed  in  Table  2-2  for  the  Puma  762  manipulator. 

A  more  complicated  problem  arises  when  one  is  trying  to  determine  all  possible 
joint-angle  configurations  for  a  specific  end-effector  position  and  orientation.  This 
non-trivial  solution  is  clearly  more  difficult,  however  due  to  a  favorable  geometry  of 
most  of  the  industrial  manipulators,  much  of  the  analysis  is  simplified.  The  approach  is 
referred  to  as  the  reverse  kinematic  analysis  and  begins  by  closing  the  link-loop  with  a 
hypothetical  member.  For  a  6-R  (6-revolute-joint)  manipulator  such  as  the  Puma  762,  this 
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results  in  a  1-degree-of-freedom  7-R  spatial  mechanism  with  the  angle  61  known  [9]. 

Detailed  derivation  of  the  solution  to  this  problem  is  provided  by  Crane  and  Duffy  [9], 
thus  only  the  final  equations  used  to  obtain  the  values  of  the  6-close-the-loop  parameters 
are  shown  below.  The  twist  angle  aix  can  be  calculated  using  Equations  2-10  and  2-11. 


clx=FS7-FSx  (2-10) 

s7l={FS7xFSx)FSx  (2-11) 

Q 

Next,  joint  angle  7  is  found  using  Equations  2-12  and  2-13. 

cn=F  a61-F  aix  (2-12) 

si  =  (F  a^Falx)Falx  (2-13) 


'Y  .  ci 

Parameter  ' 1  is  defined  as  the  angle  between  the  vector  71  and  the  x-axis  of  the  fixed 
coordinate  system,  thus  using  a  similar  approach  as  outlined  above,  this  angle  is  obtained 
using  Equations  2-14  and  2-15. 


cos;q=  a 


71 


1 
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(2-14) 
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(2-15) 


S  a  S 

Finally,  the  values  of  7 ,  71  and  ‘can  be  obtained  and  are  given  by  Equations  2-16,  2- 
17,  and  2-18  respectively. 


(2-16) 
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S,= 


(2-17) 


(2-18) 


Table  2-3  shows  the  mechanism  parameters  for  the  newly  formed  closed-loop  spatial 
mechanism. 


Table  2-3.  Closed-loop  mechanism  parameters  of  the  Puma  762  robot 


Link  Length,  mm 

Twist  Angle,  deg 

Jnt  Offset,  mm 

Joint  Angle,  deg 
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5!  =  C.L. 
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a23  =  650 

a23  =  0 
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a 
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II 

■'t 

a 

a45  =  90 

54  =  600 

0A  =  variable 

a56  ^ 

£ 

Os 

ii 

so 

o 

55=0 

6*5  =  variable 

a61  —  0 

a67  =  90 

56  =  129 

0(  =  variable 

a71  =  C.L. 

S7  =  C.L. 

e7  =c.l. 

Once  all  of  the  closed-loop  parameters  have  been  calculated,  the  vector-loop 
equation  for  the  closed-loop  Puma  mechanism  can  be  constructed  and  is  represented 
using  Equations  2-19: 

5)5)  +  S2S2  -\-  u~, 2@23  +  5454  +  5656  +  S7iS7i  +  =  0  (2-19) 


Expanding  these  vectors  and  expressing  them  in  terms  of  the  Set  14  of  the  table  of 
direction  cosines  for  the  spatial  heptagon  (Appendix  A)  results  in: 
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Further  simplification  is  obtained  by  using  subsidiary  spatial  and  polar-sine,  sine-cosine, 
and  cosine  laws  from  Equation  2-2 1 . 
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^5671 
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co 

1 _ 
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1  5671 
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L  5671  _ 

_  Z3  _ 

(2-21) 


The  representation  in  Equation  2-20  yields  three  equations.  Further  substitution  of  the 
known  terms  and  simplification  of  the  Z  component  of  the  equation  yields  Equation  2-22. 
[S6Y7  -  S7571  ]c1  +  [S6X1  +  an  K  +  [52  ]  =  0  (2-22) 


This  is  an  equation  of  the  form  +  +  ^  ^ ,  where  A,  B,  and  D  are  constants.  The 

6  6 

solution  provides  two  possible  values  labeled  as  and  ]h .  Corresponding  values  of  the 

angle  ^  can  be  calculated  as  ^la  Zl  ^  and  Zl  ^  . 

Substituting  the  known  values  into  the  X  and  Y  components  of  the  Equation  2-20 
yields  Equations  2-23  and  2-24: 

"t"  ^4-^32  ^ =  0  (2-23) 

~  ~  a23s2  +  a34^32  ~  ^4^32  +  ^6^71  +  ^7^1  =  0  (2-24) 


Moving  the  terms  that  do  not  contain  the  unknown  variables  and  expanding  the 
direction-cosines  results  in  Equations  2-25  and  2-26: 


a22C2  ^4^2+3  Z 


(2-25) 


a23S2  S4C2+j  —  B 


(2-26) 


where  ^  ‘“*6^71  ^7^1  alxcx  B  Sx  S6Y7l  ?  while  ^2+3  an(j  c2+3  repreSent 


(Q  _(_  Q  ) 

sine  and  cosine  of  v  2  3  ’ ,  respectively.  Squaring  both  sides  of  both  Equations  2-25 

and  2-26  and  then  adding  them  yields  Equation  2-27. 

a22>  ^4  2^23^4  [^2^2+3  _  C2S2+3  ]  —  Z  ^ 


(2-27) 
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where  [ s2c2+3  -  c2s2+3  ]  =  sin[<92  -  (02  +  0, )]  =  sin(-  6, )  -  -s3 .  Substituting  this  tenn  back 


into  the  Equation  2-27  results  in  Equation  2-28. 
s3  [—  2anS4  ]  +  [#23  +  S4  —  A~  —  J  =  0 


(2-28) 


Q  p 

This  equation  contains  only  3  as  the  unknown.  For  each  value  of  1 , 2  corresponding 

Q 

values  of  3  are  detennined.  Regrouping  Equations  2-25  and  2-26  yields  the  following 
relationships: 


C2[#23  *^4^3  ]”*”  ^2  [  “^4^3]  ^ 

S 2  [—  a23  +  S4S3  ]  +  C2  [—  S4C3  ]]  =  B 


(2-29) 

(2-30) 


Equations  2-29  and  2-30  provide  a  system  of  2  equations  and  2  unknowns.  Thus  a  unique 

p  P  Q 

value  of  2  is  solved  for  each  corresponding  set  of  1  and  3 .  The  next  step  is  to  solve  for 

0, 


using  Equation  2-3 1 : 


7  =7 

^  7123  3 


(2-31) 


0, 


With  corresponding  substitutions  and  expansions,  this  yields  two  values  of  5  for  each 
set  of^7 ,  ^ ,  ^3,  and  as  follows: 


C5  ~  ^7123 


(2-32) 


0, 


Solution  of  4  is  obtained  from  a  system  of  2  subsidiary  spherical  Equations  2-33 
and  2-34. 


"^54  —  "^7123 


Y  —  -V 

^  54  -'7123 


(2-33) 


(2-34) 


where  ^sc4  Y5s4  and  ^54  ^s54  +  ^C4 .  Final  form  of  the  solution  shown  in 
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0 

Equations  2-35  will  result  in  a  unique  value  of  4  for  each  of  the  8  sets  of  corresponding 
angle  values. 


c4 


(2-35) 


*4 


(2-36) 


0 

The  solution  of  the  last  remaining  joint  angle  6  is  obtained  using  the  following 
two  fundamental  spherical  sine  and  sine-cosine  laws  defined  by  Equations  2-37  and  2-38. 

^43217  =  S56S6  (2-37) 

^43217  =  S56C6  (2-38) 


Since  1X56  is  known,  the  equations  reduce  to  5  6  ^45217  andCfi  ^43217  Jhe  final  solution 

tree  for  the  Puma  762  robot  is  shown  below  in  Figure  2-3. 


07 


Figure  2-3.  Reverse  analysis  solution  tree  for  Puma  762  robot 
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2.2.3  Forward  and  Reverse  Velocity  Analysis 

Similar  to  the  forward  position  analysis,  the  velocity  analysis  assumes  that  the 
angular  velocities  of  each  of  the  joints  are  known,  and  as  a  result  it  determines  the 
velocity  state  of  the  last  link  of  the  manipulator  as  shown  in  Equations  2-39. 


"°<y6 

0-6 


=°co60S6=1co2  lS2+2a>3  2S3+3a)43S4+4a>5  4$5 +5co65$6  (2-39) 


where  ^ 1  are  the  unitized  line  coordinates  for  each  of  the  joint-axes.  From  Duffy  and 
Crane[10],  this  equation  can  be  rewritten  in  the  matrix  fonnat  yielding  Equation  2-40. 

=°(o60S6  =  Jco  (2-40) 

v 


where  J,  the  Jacobian  matrix,  is  the  6X  6  matrix  formed  from  the  unitized  screw 
coordinates,  which  can  be  written  using  Equation  2-41. 


J  =  \°S 1  lS2  2S 3  3£4  4£5  5S6]  =  _ 

°Sln 


0  nl  1  n2  2  o3  3  o  4  4  o5  5  n6 


1  1  oZ 

o 


1  o  2  2  o3  3  n4  4  n5  5  o  6 


(2-41) 


where  co  is  the  vector  of  joint  velocities  defined  by  Equation  2-42. 

ry  =  [°6U1  x(o2  2  co2  2co4  4co5  5ru6]r  (2-42) 

Assuming  that  the  position  analysis  is  completed,  all  members  of  the  Jacobian  are 
known,  thus  the  forward  velocity  analysis  is  completed  using  Equation  2-39.  Just  as  was 
the  case  with  the  position  analysis,  the  reverse  velocity  problem  becomes  more 
complicated  and  individual  joint  velocities  can  be  detennined  by  simple  arithmetic 
yielding  Equation  2-43. 

r  — 1  0  6  0  rr»6 

CO  —  J  •  CO  •  lb 


(2-43) 
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It  is  possible  that  the  manipulator  is  driven  to  a  configuration  in  which  the  Jacobian 
matrix  is  singular  and  the  inverse  cannot  be  obtained  [10].  Such  configurations  are  the 
topic  of  the  following  section. 

2.2.4  Singularity  Determination 

A  robot  is  defined  to  be  in  a  singular  configuration  when  the  determinant  of  the 
Jacobian  is  equal  to  0.  Duffy  and  Crane  [10]  also  define  this  special  configuration  when  1 
or  more  of  the  joint-axis  lines  as  defined  in  the  previous  section  become  linearly 
dependent  to  the  others.  This  section  identifies  those  configurations  in  which  such 
conditions  occur.  Such  configurations  will  cause  joint  velocities  to  rapidly  increase,  and 
there  will  be  no  feasible  solutions  of  the  manipulator  to  move  in  a  desired  direction. 

Many  computational  methods  are  developed  to  optimize  singularity  avoidance. 

For  practical  purposes,  the  line  coordinates  of  the  Jacobian  matrix  shown  in 
Equation  2-44  are  expressed  in  terms  of  the  third  coordinate  system.  This  is 
arithmetically  the  most  favorable  selection  and  allows  for  easy  identification  of  singular 
configurations. 

s2+3  0  0  0  s4  s5cA 

c2+3  0  0  1  0  -c5 

0  1  1  0  c4  —  ^  5^  4 

$2C2+3  a23S3  0  0  S4C4  ~  S4SyS4 

—  S2S2+3  a22C2  0  0  0  0 

-a22c2  0  0  0  -S4s4  -S4s5c4 

The  determinant  of  this  Jacobian  yields  Equation  2-45. 

det[/  ]  —  —a22S4  (s5  )(c3 )[—  ci22c2  +  S4s2+2  ]  (2-45) 

The  left  side  of  the  Equation  2-45  equals  zero  if: 

•  Wrist  singularity  (s5  =  0):  In  this  case  vectors  S4  and  S6  become  collinear. 


(2-44) 
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•  Forearm  boundary  singularity  [11]  (c3  =  0):  In  this  case  vectors  S2  and  a23  are 
parallel  and  the  elbow  is  fully  extended. 

•  Forearm  interior  singularity  [11]  ([-a23C2+S4S2+3]=0):  The  interpretation  of  this 
term  is  rather  complicated,  but  it  can  be  said  that  the  singularity  occurs  when  vector 
S6  is  very  close  to  the  Sl  vector. 

Figures  2-4,  2-5,  and  2-6  show  the  wrist,  forearm  interior,  and  boundary 
singularities,  respectively. 


Figure  2-4.  Wrist  singularity  of  Puma  762  robot 


Figure  2-5.  Forearm  boundary  singularity  of  Puma  762  robot 
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Singular  Directions,  -x.  -y,  and  z. 


Figure  2-6.  Forearm  interior  singularity  of  Puma  762  robot 

The  above  3  configurations  require  special  attention  and  will  be  a  matter  of  interest 
in  the  upcoming  chapters. 

2.3.5  Quaternion  Representations 

Crane  and  Duffy  [9]  define  a  real  quaternion  as  a  set  of  4  real  numbers  written  in 
definite  order  as  shown  in  Equation  2-46. 


q  =  ( d,a,b,c ) 


(2-46) 


Chapter  6  will  use  quaternion  representation  for  the  position  and  orientation  of  the 
end-effector.  For  that  reason,  this  section  defines  those  aspects  of  the  quaternion  algebra. 
The  unit  quaternion  q  is  defined  in  Equation  2-47. 


(2-47) 


The  equivalent  rotation  matrix  is  defined  by  Crane  and  Duffy  [9]  as  the  following: 


s2x  (1  -  cos 26*)  +  cos  20  sxsy  (1  -  cos 20)  -  s,  sin  20  sxsz  (1  -  cos 20)  +  sy  sin  20 
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In  summary,  this  chapter  provides  all  the  necessary  analysis  sufficient  for  tasks 
mandated  by  the  proposed  JAUS  components.  Equations  derived  in  Sections  2.3.3  and 
2.3.4  will  become  essential  for  component  development  outlined  in  Chapters  5,  6  and  7. 
Detailed  information  on  theory  covered  in  this  chapter  and  more  advanced  robot 
manipulator  analysis  is  presented  by  Crane  and  Duffy  [9]  and  Duffy  [10]. 


CHAPTER  3 

PUMA  762  CONTROLLER  SYSTEM 

This  chapter  addresses  the  capabilities  of  an  onboard  controller  system  and  ties 
them  into  the  requirements  outlined  by  the  JAUS  Compliance  Specification  (JCS) 
document.  Before  modular  architecture  implementation,  the  platform  was  in  operational 
condition,  allowing  the  user  to  control  the  motion  of  the  individual  joints  using 
commercially  available  software.  Such  conditions  required  no  additional  changes  to  the 
current  hardware  configuration.  For  completion  purposes,  this  chapter  provides  a  brief 
summary  of  modifications  to  the  original  system  configuration.  Further  emphasis  is  put 
on  the  functions  available  in  the  C/C++  Application  Programming  Interface  (API)  and  its 
use  in  this  implementation. 

3.1  Overview 

Native  to  the  Puma  762  is  the  Val  II  high  level  programming  language.  Aside  from 
being  a  sophisticated  development  tool,  Val  II  is  a  complete  control  system.  Joint 
Architecture  for  Unmanned  Systems  standard  currently  dictates  the  use  of  a  C 
programming  language  for  development  purposes.  Due  to  some  deficiencies  in  the  older 
VAL  controller  and  lack  of  an  adequate  API,  the  Puma  was  outfitted  with  a  Galil  DMC- 
2100  motion  controller.  Unfortunately,  the  use  of  a  commercially  available  motion 
controller  dictates  operational  constraints.  The  JCS  document  mentioned  above  defines 
three  levels  of  interoperability.  This  implementation  satisfies  inter-nodal,  or  level  II 
compliance,  supporting  interoperation  between  nodes  [12].  Figure  3-1  shows  the  simplest 
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representation  of  the  overall  system.  Connectivity  between  Val  and  Galil  is  addressed  in 
the  following  section. 


Figure  3-1.  Schematic  representation  entire  manipulator  system 

3.2  Reverse  Engineering  the  Puma  762  Robot 
Hardware  interfacing  between  the  Val  and  Galil  controllers  was  done  by  Prof. 
Robert  Bicker  of  the  University  of  Newcastle  upon  Tyne  in  June  of  2001.  This  section 
summarizes  all  the  necessary  modifications  made  to  effectively  outfit  the  system  with  a 
newer  Galil  controller  while  maintaining  some  of  the  basic  Val  functionality. 

3.2.1  Existing  Architecture 

Val  II  is  a  hierarchical  controller  based  on  the  LSI  1 1-73  microcomputer  that 
provides  a  high-level  trajectory  and  program  control  of  the  robot  using  the  native 
programming  language.  Joint  proportional-integral-derivative  (PID)  control  is  provided 
by  dedicated  digital  axis  servo-boards  (using  an  Intel  8748  microcontroller)  running  at  1 
kHz.  The  12-bit  digital-to-analog  converter  (DAC)  output  of  the  digital  servo-boards  is 
input  to  the  individual  PWM  power  amplifiers  (PAs).  Three  major  PAs  are  located  on  the 
rear-door  of  the  controller,  while  three  minor  ones  are  located  in  the  control  rack.  As 
mentioned  earlier,  each  joint  of  the  robot  is  driven  by  a  direct-current  (DC)  servo-motor 
with  integral  electro-magnetic  brake.  Mounted  directly  on  the  end  of  the  motor  shaft  are 
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an  incremental  encoder  (250  lines)  and  a  geared  servo-potentiometer  providing  position 
feedback.  The  initial  calibration  process  requires  each  joint  to  be  driven  through  a  small 
angle,  sufficient  to  locate  the  nearest  encoder  index.  This  non-absolute  position  is  then 
compared  to  the  values  stored  in  a  look-up  table  via  the  corresponding  analog 
potentiometer  reading  [13]. 

3.2.2  Encoder  and  Potentiometer  Val  Interface 

The  board  edge  connector  located  in  the  J56  slot  directs  encoder  quadrature  (A  & 
Q),  index  and  potentiometer  outputs  to  respective  digital  servo-boards  (J45-50)  via  a 
J-bus  backplane.  Wire-wrap  terminals  are  provided  at  the  rear  of  the  backplane  only  on 
the  J56,  and  wire-wrap  connections  are  made  to  a  50-way  IDC  wire-wrap  connector.  A 
50-way  ribbon  cable  runs  to  a  break-out  box,  and  then  single  core  cables  are  bundled  to 
the  respective  connections  on  the  Galil  ICM-2900  modules,  designated  1  and  2  (Figure 
3-2). 


Figure  3-2.  Arm  signal  interconnects  between  Val  and  ICM-2900  modules 
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3.2.3  Amplifier  Digital-to-Analog  Converter  Signals  and  Control  Lines 

The  DAC  outputs  on  the  Val  controller  for  each  axis  are  derived  by  the  digital 
servo  boards.  The  DAC  (+/-)  signals  are  presented  to  a  34-way  IDC  connector  on  the 
control  backplane,  and  are  linked  to  the  power  amplifier  backplane  via  a  34-way  ribbon 
cable  (J103  =>  J79).  Additional  control  lines  are  also  present.  Table  3-1  shows  the  pin 
connections  between  the  Val  controller  and  Galil’s  interconnect  modules. 

Table  3-1.  Pin  connections  between  Val  and  Galil  ICM-2900  interconnect  modules 


VAL  Signal 

34/50  way  pin 

Wire  color 

ICM-2900 

teach-error 

1 

white 

stop 

2 

white 

1/1 4/INPUT  1 

Hnd-sigl 

3 

white 

Hnd-sig2 

4 

white 

Brk-on-hi 

5 

white 

Deadman-sw 

6 

white 

E-stop  b 

7 

white 

ox-inhibit 

8 

white 

1/14/INPUT3 

Servo  E-stop 

9 

white 

DAC+1 

11 

orange 

1/3/MOCMDX 

DAC-1 

12 

blue 

1/3/GND 

DAC +2 

13 

orange 

1/4/MOCMDY 

DAC -2 

14 

blue 

1/4/GND 

DAC+3 

15 

orange 

1/1/MOCMDZ 

DAC -3 

16 

blue 

1/1/GND 

DAC +4 

17 

orange 

1/2/MOCMDW 

DAC -4 

18 

blue 

1/1/GND 

DAC+5 

19 

orange 

2/3/MOCMDX 

DAC -5 

20 

blue 

2/3/GND 

DAC+6 

21 

orange 

1/4/MOCMDY 

DAC -6 

22 

blue 

1/4/GND 

Remote  +ve 

33 

white 

Encoder  fault 

34 

white 

1/14/INPUT2 
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In  order  to  retain  Val  safety  functionality,  all  control  lines  are  connected  straight 
through  via  the  34-50-50-34  interconnection  cabling.  Stop,  Encoder-fault  and  Ox-inhibit 
signals  are  linked  to  digital  inputs  1-3,  respectively  of  the  ICM  module  1.  The  amplifier 
enable  signal  is  provided  by  a  single  control  line  BRK-ON-HI  (brake  on  high).  This  is  a 
TTL  open-collector  output  from  the  LSI-1 1  controller,  which  is  pulled  low  on  VAL  being 
initialized.  A  7406  hex-inverter  (with  open-collector  outputs)  is  used  to  drive  this  line 
low  via  the  Galil  digital  output  1  as  is  shown  in  Ligure  3-3. 


+5v 


1 

1 

I 

1/8/OUT1 

1 

r\2_ 

1 

BRK-ON-HI  | 

- 

- 

7406 

i 

i 

i 

jvAL 

i 

Ligure  3-3.  BRK-ON-HI  connection  diagram 

3.2.4  Safety 

The  power  amplifiers  are  enabled  by  pulling-low  BRK-ON-HI  using  the  Galil 
command  OP1  (OutPut  1  set).  Ann  power  is  applied  using  the  remote  ON/OLL  switch 
box  (if  motion  control  set  to  REMOTE)  or  via  the  front  panel  (if  set  to  LOCAL).  The  ann 
will  attempt  to  go  into  closed  loop  at  this  time,  and  it  is  essential  to  apply  either  the  SH  or 
MO  (ServoHere,  MotorOff)  Galil  commands  prior  to  powering  up  the  ann  to  prevent  any 
run- away. 

3.2.5  Tuning 

Axis  tuning  was  carried  out  sequentially,  initially  in  the  digital  feedback  mode 
(ALn=0),  and  subsequently  in  the  analog  feedback  mode  (AFn=l).  Step  response  tests 
were  canied  out  using  the  Servo-Design  Kit  software.  Small  and  large  steps  (100  and 
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1 000  counts  -  using  encoder  feedback)  as  well  as  manual  tuning  and  auto-tuning  features 
were  used,  although  manual  tuning  was  preferred.  Proportional  (KPn),  Derivative  (KDn) 
and  Integral  action  gains  (Kin)  are  set  as  shown  in  Table  3-2. 


Table  3-2.  Controller  gain  values 


Axis/Joint  # 

KP 

KD 

KI 

A/1 

80 

500 

1 

B/2 

60 

500 

1 

C/3 

100 

200 

1 

D/4 

100 

500 

1 

E/5 

100 

500 

1 

F/6 

100 

500 

1 

Once  adequately  tuned,  this  configuration  provides  a  user  friendly  computer 
interface  and  allows  for  simple  control  of  the  manipulator. 

3.3  Galil  DMC-2100  Functionality 

DMC-2100  is  one  of  Galil’s  highest  performance  stand-alone  controllers  outfitted 
with  many  enhanced  features  including  high-speed  communications,  non-volatile 
program  memory,  faster  encoder  speeds  and  improved  cabling  for  noise  reduction.  It  is 
designed  to  solve  complex  problems  involving  jogging,  point-to-point  positioning,  vector 
positioning,  electronic  gearing,  multiple  move  sequences,  and  contouring  [14]. 

3.3.1  Command  Modes 

There  are  several  of  ways  to  command  motion  using  the  Galil  controller.  The 
requirements  of  this  project  are  simple  and  only  take  advantage  of  two  motion  modes. 
Independent  axis  positioning  generates  an  independent  trajectory  profile  for  each  of  the 
axes.  The  user  commands  either  absolute  or  relative  position  and  selects  the  values  of 
maximum  speed,  acceleration  and  deceleration.  The  second,  and  more  relevant,  is 
independent  jogging.  This  mode  is  very  flexible  and  allows  the  user  to  change  all  of  the 
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above-mentioned  values  during  motion  and  specify  the  direction.  The  controller  operates 
as  a  closed-loop  position  controller  while  in  the  jog  mode.  It  converts  the  velocity  profile 
into  a  position  trajectory  and  a  new  position  target  is  generated  with  every  sample  period 
[14]. 

3.3.2  Theory  of  Operation 

This  section  only  offers  a  brief  overview  of  the  operation  of  a  motion  control 
system.  Detailed  information  can  be  found  in  many  control  theory  books  and  controller 
manuals.  The  operation  of  a  system  (such  as  the  DMC-2100)  could  be  divided  in  three 
levels  as  follows: 

•  Closing  the  loop 

•  Motion  profiling 

•  Motion  programming. 

Figure  3-4  shows  the  elements  of  the  servo  system.  On  the  lowest  level,  the 
controller  ensures  that  the  motor  follows  the  commanded  position.  This  is  accomplished 
using  a  feedback  provided  by  a  sensor.  In  the  case  of  the  Puma  system,  position  feedback 
is  provided  by  an  incremental  encoder. 


CONTROLLER 


Figure  3-4.  Functional  elements  of  a  motion  control  system 

Most  of  the  commercially  available  motion  controllers  use  trajectory  generators. 
This  function  describes  where  the  motor  should  be  at  every  sampling  period.  Finally,  at 
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the  highest  level  of  control,  the  program  describes  the  tasks  such  as  desired  distances  or 
speed  [14].  A  detailed  mathematical  model  of  each  of  the  various  components  presented 
below  can  be  found  in  [14]. 

3.4  Galil  C/C++  Application  Programming  Interface  (API) 

Engineers  at  Galil  Motion  Control  Inc.  have  developed  a  set  of  API  function  calls 
that  can  be  used  in  software  development  on  various  computer  platforms.  These  calls 
provide  an  effective  link  between  the  Galil  machine  code  and  standard  C  or  C++ 
programming  languages.  Manipulator  JAUS  implementation  more  specifically  requires 
libraries  running  on  the  Linux  operating  system.  This  section  lists  only  the  calls  and 
corresponding  prototypes  used  in  our  study  (Figure  3-5  through  3-9).  Additional  function 
calls  and  installation  procedures  are  outlined  in  the  interface  document  [15]. 


Function  Prototype: 

extern  LONG  FAR  GALILCALL  DMCInitLibrary(void); 
Function  Description: 

This  function  must  be  called  prior  to  using  the  library. 
Figure  3-5.  Initializing  the  Galil  libraries 


Function  Prototype: 

extern  LONG  FAR  GALILCALL  DMCOpen(PCONTROLLERINF O 

_ pcontrollerinfo,  PHANDLE  phdmc); _ 

Function  Description: 

The  handle  to  the  controller  is  returned  in  the  argument  phdmc. 


Pcontrollerinfo:  Structure  holding  infonnation  about  the  controller. 

Phdmc:  Buffer  to  recelive  the  to  the  Galil  controller  to  be  used  in  all  subsequent 
calls. 


Figure  3-6.  Establishing  communications  with  the  Galil  Controller 


Function  Prototype: 

extern  LONG  FAR  GALILCALL  DMCClose(HANDLEDMC  hdmc); 
Function  Description: 

This  function  closes  the  Ethernet  connection  to  the  controller. 


Figure  3-7.  Closing  the  connection  to  the  Galil  Controller 
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Function  Prototype: 

extern  LONG  FAR  GALILCALL  DMCCommand(HANDLEDMC  hdmc,  PSZ 

_ pszCommand,  PCFIAR  pchResponse,  ULONG  cbResponse); _ 

Function  Description: 

This  function  is  used  to  send  commands  to  the  controller. 


pszCommand:  The  command  send  to  the  controller  in  the  machine  language. 
pchResponse:  Buffer  receiving  the  response  data. 
cbResponse:  Length  of  the  buffer. 


Figure  3-8.  Sending  commands  to  the  Galil  Controller 


Function  Prototype: 

extern  LONG  FAR  GALILCALL  DMCReset(HANDLEMC  hdmc); _ 

Function  Description: 

This  function  is  used  to  reset  the  controller. 

Figure  3-9.  Resetting  the  controller 

The  use  of  these  calls  will  become  more  apparent  in  Chapter  9  as  the  aspects  of 
software  design  on  a  node  level  are  introduced. 


CHAPTER  4 

LOW-LEVEL  MANIPULATOR  CONTROL  COMPONENT 
This  chapter  defines  the  Primitive  Manipulator  (PM)  component  in  tenns  of  the 
JAUS  architecture.  Lor  the  purposes  of  easy  insertion,  the  format  used  to  present  the 
information  in  the  first  section  of  this  and  the  upcoming  chapters,  follows  the  latest 
format  outlined  in  version  3.2  of  the  JAUS  Reference  Architecture  Document.  The 
chapter  further  details  the  application  of  the  PM  to  the  Puma  system. 

4.1  Primitive  Manipulator  Component 

The  one  component  in  this  category  (JAUS  ID#  49)  allows  for  low-level  command 
of  the  manipulator  joint  efforts.  This  is  an  open-loop  command  that  could  be  used  in  a 
simple  tele-operated  scenario.  This  component  is  currently  in  version  3.2  of  the  JAUS 
Reference  Architecture. 

4.1.1  Definition  of  Coordinate  Systems 

A  variety  of  platform  configurations  on  which  manipulator  components  can  be 
implemented  might  require  data  to  be  interpreted  in  terms  of  different  coordinate  systems 
either  to  adequately  define  the  geometry  or  allow  for  easier  task  completion. 

4.1. 1.1  Global  coordinate  system 

Points  that  are  defined  in  this  coordinate  system  are  defined  in  units  of  longitude, 
latitude,  and  elevation.  The  x-axis  points  North  and  the  z-axis  points  downward. 

4.1. 1.2  Vehicle  coordinate  system 

This  coordinate  system  is  attached  to  the  vehicle  frame.  The  x-axis  points  in  the 
forward  direction  and  the  z-axis  points  downward.  The  y-axis  is  defined  so  as  to  have  a 
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right-handed  coordinate  system  (i.e.  i  x  j  =  k  where  i,  j,  and  k  are  unit  vectors  along  the 
x,  y,  and  z  coordinate  axes). 

4.1. 1.3  Manipulator  base  coordinate  system 

In  most  cases,  this  coordinate  system  is  attached  to  the  vehicle  base  just  like  the 
vehicle  coordinate  system.  The  origin  is  located  at  the  intersection  of  the  line  along  the 
first  joint  axis,  Si,  and  the  line  along  the  first  link,  ai2.  The  z-axis  is  along  Si  and  the 
direction  of  the  x-axis  is  user  defined,  but  fixed  with  respect  to  the  vehicle  frame.  Since 
the  manipulator  base  coordinate  system  and  the  vehicle  coordinate  system  are  both 
attached  to  the  vehicle  base,  the  transformation  matrix  that  describes  the  relative  position 
and  orientation  of  these  two  coordinate  systems  will  be  constant.  In  some  cases  where 
one  manipulator  may  be  attached  to  the  end  of  another  manipulator,  this  transformation 
matrix  would  vary. 

4.1. 1.4  End-effector  coordinate  system 

This  coordinate  system  is  attached  to  the  last  link  of  the  serial  manipulator.  The 
origin  is  located  at  the  point  that  is  a  distance  Sn  (S6  for  a  six  axis  manipulator)  along  the 
last  joint  axis  vector  (S6  for  a  six  axis  manipulator)  from  the  intersection  of  the  lines 
along  the  last  joint  axis  and  the  preceding  link  axis  (S6  and  a 56  for  a  six  axis  manipulator). 
The  Z  axis  is  along  the  Sn  vector.  The  direction  of  the  x-axis  is  user  defined,  but  of 
course  must  be  perpendicular  to  the  z-axis.  The  y-axis  is  defined  by  the  right-hand  rule. 
Note  that  the  distance  Sn  is  returned  by  the  Primitive  Manipulator  component  via  the 
Report  Manipulator  Specifications  Message. 

4.1.2  Component  Function 

This  component  is  concerned  only  with  the  remote  operation  (open-loop  control)  of 
a  single  manipulator  system.  The  manipulator  may  or  may  not  have  joint  measurement 
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sensors  that  would  provide  joint  position  and  joint  velocity  infonnation.  Without  joint 
measurement  sensors,  the  operator  can  only  send  joint  motion  efforts  as  commands  to  the 
manipulator.  This  component  does  not  use  any  joint  angle  or  velocity  feedback  from  the 
manipulator.  It  only  receives  the  desired  percentage  of  maximum  joint  effort  for  each 
joint  as  an  input. 

4.1.3  Associated  Messages 

The  PM  accepts  the  core  input  and  output  messages  listed  in  Table  1-1.  The  set  of 
user  defined  input  and  output  messages  specific  to  this  component  are  listed  below. 
Detailed  definitions  are  provided  in  Section  4.2.6. 

•  Set  Joint  Effort 

•  Query  Manipulator  Specifications 

•  Query  Joint  Efforts 

•  Report  Manipulator  Specifications 

•  Report  Joint  Efforts 

4.1.4  Component  Description 

This  component  is  the  low-level  interface  to  a  manipulator  ann  and  is  in  many 
respects  similar  to  the  Primitive  Driver  component  for  mobility  of  the  platform.  When 
queried,  the  component  will  reply  with  a  description  of  the  manipulator’s  specification 
parameters,  axes  range  of  motion,  and  axes  velocity  limits.  The  notations  used  to 
describe  these  data  are  documented  in  many  popular  text  books  on  robotics  and  were 
previously  presented  in  Chapter  2.  The  mechanism  specification  parameters  as  reported 
by  the  Report  Manipulator  Specifications  Message  consist  of  the  number  of  joints,  the 
type  of  each  joint  (either  revolute  or  prismatic),  the  link  description  parameters  for  each 
link  (link  length  and  twist  angle,  Figures  1-1  to  1-3),  the  constant  joint  parameter  value 
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(offset  for  a  revolute  joint,  Figure  1-2),  and  joint  angle  for  a  prismatic  joint  (Chapter  1, 
Figure  1-3).  The  minimum  and  maximum  allowable  value  for  each  joint  and  the 
maximum  velocity  for  each  joint  follow  this  information.  Motion  of  the  arm  is 
accomplished  via  the  Set  Joint  Effort  message  (Figure  4-1).  In  this  message,  each 
actuator  is  commanded  to  move  with  a  percentage  of  maximum  effort. 


’ 

Commanded  joint  effort 

r 

Primitive 

Manipulator 

' 

Actuator  commands 

r> 

Figure  4-1.  Joint  effort  provides  basic  manipulator  mobility 

4.1.5  Input  and  Output  Messages 

This  section  provides  detailed  specifications  of  user  defined  messages.  It  covers 
data  types  used  for  each  of  the  parameters,  as  well  as  the  upper  and  lower  limits.  This 
information  becomes  very  important  for  software  development  purposes  and  is  further 
covered  in  Chapter  8. 

4.1. 5.1  Code  0601h:  Set  Joint  Effort 

Field  #1  in  Table  4-1  indicates  the  number  of  joint  effort  commands  contained  in 
this  message.  This  message  sets  the  desired  joint  effort  values. 


Table  4-1.  Set  Joint  Effort  message  parameters 


Field  # 

Name 

Type 

Units 

Interpretation 

1 

Num  Joints 

Byte 

N/A 

1  ...  255;  0  is  Reserved 

Scaled  integer 

2 

3  ...  n 

Joint  1  -effort 

Short  int 

Percent 

Lower  Limit  =  -100% 

Upper  Limit  =  +100% 

n  +  1 

Joint  n  -effort 

Short  int 

Percent 

see  field  2 
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4.1. 5.2  Code  2600h:  Query  Manipulator  Specifications 

This  message  shall  cause  the  Primitive  Manipulator  component  to  reply  to  the 
requestor  with  a  Code  4600h:  Report  Manipulator  Specifications  message. 

4.1. 5.3  Code  2601h:  Query  Joint  Effort 

This  message  shall  cause  the  receiving  component  to  reply  to  the  requestor  with  a 
Code  460 lh:  Report  Joint  Efforts  message. 

4.1. 5.4  Code  4600h:  Report  Manipulator  Specifications 

This  message  provides  the  specifications  of  the  manipulator  including  the  number 
of  joints,  the  link  lengths,  twist  angles,  offset  or  joint  angles,  minimum  and  maximum 
value  for  each  joint,  and  minimum  and  maximum  speed  for  each  joint. 

4.1. 5.5  Code  4601h:  Report  Joint  Effort 

This  message  provides  the  receiver  with  the  current  values  of  the  commanded  joint 
effort.  The  message  data  and  mapping  of  the  presence  vector  for  the  Report  Joint  Efforts 
message  are  identical  to  Code  060 lh:  Set  Joint  Effort. 
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Table  4-2.  Report  Manipulator  Specifications  parameters 


Field  # 

Name 

Type 

Units 

Interpretation 

1  . . .  255  0  is  Reserved 

1 

Number  of 

Joints 

Byte 

N/A 

For  a  value  of  n  the  message 
data  area  size  in  bytes  is: 

(36  +  1 3 (n- 1 ))  bytes. 

2 

Joint  n  Type 

Byte 

N/A 

Joint  type  of  the  last  joint  of  the 
manipulator. 

1  =  revolute,  2  =  prismatic 

3 

Joint  n  - 
Offset/Joint 

Unsigned 

Short 

rad  or 

Prismatic  joint, 
value  x  10'3  radians. 

Angle 

mm 

Revolute  joint,  mm 

4 

5 

Joint  n  - 
Min  value 

Joint  n  - 
Max  value 

Unsigned 

Short 

rad  or 

mm 

Prismatic  joint,  mm 

Revolute  joint, 
value  x  10"3  radians. 

6 

Joint  n  - 
Max  velocity 

Unsigned 

Short 

rad/s  or 
mm/s 

Prismatic  joint,  mm/sec 

Revolute  joint, 
value  x  1 0"3  radians/sec. 

7 

manipulator 
coordinate 
sys.  x 

Integer 

m 

x  coordinate  of  origin  of 
manipulator  coordinate 
system  measured  with  respect 
to  vehicle  coordinate  system 
Scaled  integer: 

Lower  limit  =  -30  m 

Upper  limit  =  +30  m 

8 

manipulator 
coordinate  sys. 

y 

Integer 

m 

y  coordinate  of  origin  of 
manipulator  coordinate 
system  measured  with  respect 
to  vehicle  coordinate  system 

9 

manipulator 
coordinate 
sys.  z 

Integer 

m 

z  coordinate  of  origin  of 
manipulator  coordinate 
system  measured  with  respect 
to  vehicle  coordinate  system 

10 

d  component  of 
unit  quaternion  q 

Integer 

N/A 

quaternion  q  defines  the 
orientation  of  the  manipulator 
coordinate  system  measured 
with  respect  to  the  vehicle 
coordinate  system 

Scaled  integer: 

Lower  limit  =  - 1 

Upper  limit  =  +1 
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Table  4-2.  Continued 


Field  # 

Name 

Type 

Units 

Interpretation 

12 

b  component  of 
unit  quaternion  q 

Integer 

N/A 

see  field  10 

13 

c  component  of 
unit  quaternion  q 

Integer 

N/A 

see  field  10 

14 

Joint  1  Type 

Byte 

N/A 

1  =  revolute,  2  =  prismatic 

14a 

Link  a12  - 
Link  Length 

Unsigned 

Short 

mm 

Link  Length 

14b 

Link  a12  - 
Twist  Angle 

Unsigned 

Short 

rad 

Twist  Angle  - 
value  x  1 0"3  radians 

14c 

Joint  1  - 
Offset/Joint 

Unsigned 

Short 

rad  or 

Prismatic  joint, 
value  x  10"3  radians. 

Angle 

mni 

Revolute  joint,  mm 

14d 

14e 

Joint  1  - 
Min  value 

Joint  1  - 
Max  value 

Unsigned 

Short 

rad  or 

mm 

Prismatic  joint,  mm 

Revolute  joint, 
value  x  10"3  radians. 

14f 

Joint  1  - 
Max  velocity 

Unsigned 

Short 

rad/s  or 
mm/s 

Prismatic  joint,  mm/sec 

Revolute  joint, 
value  x  1 0"3  radians/sec. 

n-1 

Joint  (n-1)  Type 

Byte 

N/A 

1  =  revolute,  2  =  prismatic 

(n-l)a 

Link  a(n_1)n  - 
Link  Length 

Unsigned 

Short 

mm 

Link  Length 

(n-l)b 

Link  a(n_1)n  - 
Twist  Angle 

Unsigned 

Short 

rad 

Twist  Angle  - 
value  x  1 O'3  radians 

(n-l)c 

Joint  (n-1)  - 
Offset/Joint 
Angle 

Unsigned 

Short 

rad  or 

mm 

Prismatic  joint, 
value  x  10"3  radians. 

Revolute  joint,  mm 

(n-l)d 

(n-l)e 

Joint  (n-1)  - 
Min  value 

Joint  (n-1)  - 
Max  value 

Unsigned 

Short 

rad  or 

mm 

Prismatic  joint,  mm 

Revolute  joint, 
value  x  10"3  radians. 

(n-l)f 

Joint  (n-1)  - 
Max  velocity 

Unsigned 

Short 

rad/s 

Prismatic  joint,  mm/sec 

Revolute  joint, 
value  x  10"3  radians/sec. 
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4.2  Primitive  Manipulator  Applications  to  the  Puma  system 

As  mentioned  earlier,  the  primary  function  of  this  component  is  to  allow  for  tele-op 
commands  of  joint  efforts.  This  implementation  interprets  joint  efforts  as  the  percentages 
of  maximum  velocity.  Thus,  a  Set  Joint  Effort  message  commanding  values  of  [50,  50, 
50,  50,  50,  50]  would  in  effect  rotate  each  of  the  joints  at  fifty  percent  of  the  maximum 
velocity  in  the  positive  direction.  Note  that  the  interpretation  of  joint  efforts  can  vary 
depending  on  the  type  of  the  controller  provided.  Since  the  component  assumes  that  no 
sensor  feedback  is  provided,  it  is  possible  to  drive  the  robot  into  an  unfavorable  state 
triggering  a  fatal  Val  II  error.  Either  one  of  the  joints  reaching  a  hardware  limit,  or 
driving  a  manipulator  to  a  singular  configuration  would  cause  Val  to  respond  in  such  a 
way.  In  order  to  prevent  this  from  happening  during  the  tasks  requiring  closed-loop 
control,  this  implementation  takes  advantage  of  the  onboard  position  ad  velocity  sensor 
components  covered  in  the  following  chapter.  Computer  logic  and  software  design  of  this 
and  all  other  components  will  be  the  topic  of  Chapter  8. 


CHAPTER  5 

MANIPULATOR  SENSOR  COMPONENTS 
This  chapter  describes  the  components  that  when  queried,  return  instantaneous  joint 
position,  velocity,  and  force  or  torque  information.  Note  that  a  particular  manipulator 
might  only  have  one  or  two  measurement  sensors  onboard.  Consequently,  the 
implementation  of  all  three  components  covered  in  this  chapter  is  seldom  necessary. 
Moreover,  the  JAUS  Reference  Architecture  is  not  concerned  with  the  type  of  the  sensors 
used,  but  rather  with  the  format  in  which  the  measured  information  is  exchanged  with  the 
component  requesting  it. 

5.1  Manipulator  Joint  Position  Sensor  Component 

5.1.1  Component  Function 

The  function  of  the  Manipulator  Joint  Position  Sensor  Component  (MJPS)  is  to 
report  the  values  of  manipulator  joint  parameters  when  queried. 

5.1.2  Associated  Messages 

The  MJPS  (JAUS  ID#  5 1)  accepts  the  core  input  and  output  messages  (Chapter  1, 
Table  1-1).  The  set  of  user  defined  input  and  output  messages  specific  to  this  component 
are: 

•  Query  Joint  Positions 

•  Report  Joint  Positions. 

Detailed  definitions  are  provided  in  Section  5.1.4. 
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5.1.3  Component  Description 

The  Report  Joint  Positions  message  provides  the  instantaneous  joint  positions.  The 
positions  are  given  in  radians  for  revolute  and  in  meters  for  prismatic  joints.  The 
component  is  shown  in  Figure  5-1. 


actual  joint  positions  ; 
Report  Joint  Positions  A, 
Message 


request  joint  positions  ; 
Query  Joint  Positions 
Message 


Joint  Position 
Sensor 


Figure  5-1.  Joint  position  sensor  component 

5.1.4  Input  and  Output  Messages 

Similar  to  Section  4. 1 .5,  this  section  and  Sections  5.2.4  and  5.3.4,  are  used  to 
provide  detailed  specifications  of  user  defined  messages.  They  cover  data  types  and  value 
limits  for  each  of  the  parameters. 

5. 1.4.1  Code  0602h:  Set  Joint  Positions  message 

Set  Joint  Positions  message  is  not  defined  by  this  component  but  it  is  included  as  a 
reference. 


Table  5-1.  Set  Joint  Positions  message  parameters 


Field  # 

Name 

Type 

Units 

Interpretation 

1 

Number  of  Joints 

Byte 

N/A 

1  ...  255 

2 

Joint  1  -position 

Int 

rad  or  m 

0  is  Reserved 

Scaled  integer: 

3  ...  n 
n  +  1 

Joint  n  -position 

Int 

rad  or  m 

If  revolute  joint, 

Lower  limit  =  -871  rad 

Upper  limit  =  +8tt:  rad 

If  prismatic  joint, 

Lower  limit  =  -10  111 

Upper  limit  =  +10  m 

see  field  2 
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5.1.4.2  Code  2602h:  Query  Joint  Positions  message 

This  message  shall  cause  the  receiving  component  to  reply  to  the  requestor  with  a 
Code  4602h:  Report  Joint  Positions  message. 

5.1.4.3  Code  4602h:  Report  Joint  Positions  message 

This  message  provides  the  receiver  with  the  current  values  of  the  joint  positions. 
The  message  data  for  the  Report  Joint  Positions  message  is  identical  to  Code  0602h:  Set 
Joint  Positions. 

5.2  Manipulator  Joint  Velocity  Sensor  Component 

5.2.1  Component  Function 

The  function  of  the  Manipulator  Joint  Velocity  Sensor  (MJVS)  is  to  report  the 
values  of  instantaneous  joint  velocities  when  queried. 

5.2.2  Associated  Messages 

The  MJVS  (JAUS  ID#  52)  accepts  the  core  input  and  output  messages  (Chapter  1, 
Table  1-1).  The  set  of  user  defined  input  and  output  messages  specific  to  this  component 
are: 

•  Query  Joint  Velocities 

•  Report  Joint  Velocities. 

Detailed  definitions  are  provided  in  Section  5.2.4. 

5.2.3  Component  Description 

The  Report  Joint  Velocities  message  provides  the  instantaneous  joint  velocities. 
The  velocities  are  given  in  radians/sec  for  revolute  and  in  meters/sec  for  prismatic  joints. 
The  component  is  depicted  in  Figure  5-2. 
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actual  instantaneous  joint 
velocities  ; 

Report  Joint  Velocities  Message 


request  instantaneous  joint 
velocities  ; 

Query  Joint  Velocities  Message 


▼ 


Joint  Velocity 
Sensor 


Figure  5-2.  Joint  velocity  sensor  component 

5.2.4  Input  and  Output  Messages 

5.2.4.1  Code  0603h:  Set  Joint  Velocities  message 

Set  Joint  Velocities  message  is  not  defined  by  this  component  but  it  is  included  for 
reference  purposes. 


Table  5-2.  Set  Joint  Velocities  message  parameters 


Field  # 

Name 

Type 

Units 

Interpretation 

1 

Num  Joints 

Byte 

N/A 

1  ...255 

0  is  Reserved 

2 

3  ...  n 

Joint  1  -velocity 

int 

rad/sec  or 
m/sec 

If  revolute  joint, 

Lower  limit  =  -IOti  rad/sec 
Upper  limit  =  +107t  rad/sec 
If  prismatic  joint, 

Lower  limit  =  -5  m/sec 
Upper  limit  =  +5  m/sec 

n  +  1 

Joint  n  -velocity 

int 

rad/sec  or 
m/sec 

see  field  2 

5. 2.4. 2  Code  2603h:  Query  Joint  Velocities  message 

This  message  shall  cause  the  receiving  component  to  reply  to  the  requestor  with  a 

Code  4603h:  Report  Joint  Velocities  message. 

5.2.4.3  Code  4603h:  Report  Joint  Velocities  message 

This  message  provides  the  receiver  with  the  current  values  of  the  joint  velocities. 
The  message  data  for  the  Report  Joint  Velocities  message  is  identical  to  Code  0603h:  Set 


Joint  Velocities. 
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5.3  Manipulator  Joint  Force/Torque  Sensor  Component 

5.3.1  Component  Function 

The  function  of  the  Manipulator  Joint  Force/Torque  Sensor  (MJFTS)  is  to  report 
the  values  of  instantaneous  torques  (for  revolute  joints)  and  forces  (for  prismatic  joints) 
that  are  applied  at  the  individual  joints  of  the  manipulator  kinematic  model  when  queried. 

5.3.2  Associated  Messages 

The  MJFTS  (JAUS  ID#  53)  accepts  the  core  input  and  output  messages  (Chapter  1, 
Table  1-1).  The  set  of  user  defined  input  and  output  messages  specific  to  this  component 
are  listed  below,  while  detailed  definitions  are  provided  in  Section  5.3.4: 

•  Query  Joint  Force/Torques 

•  Report  Joint  Force/Torques 

5.3.3  Component  Description 

The  Joint  Force/Torque  Sensor  component  provides  the  instantaneous  joint  forces 
or  torques  acting  on  each  joint  of  the  manipulator  kinematic  model.  Forces  are  returned 
for  prismatic  joints  in  units  of  Newton’s  (N).  Torques  is  returned  for  revolute  joints  in 
units  of  Newton-meters  (Nm). 

5.3.4  Input  and  Output  Messages 

5.3.4.1  Code  2605:  Query  Joint  Force/Torques 

This  message  shall  cause  the  receiving  component  to  reply  to  the  requestor  with  a 
Code  4605h:  Report  Joint  Force/Torques  message. 

5.3.4.2  Code  4605h:  Report  Joint  Force/Torques 

This  message  replies  to  the  requestor  with  the  values  of  current  joint  forces  or 
torques  for  prismatic  and  revolute  joints,  respectively. 
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Table  5-3.  Report  Joint  Force/Torque  message  parameters 


Field  # 

Name 

Type 

Units 

Interpretation 

1 

Num  Joints 

Byte 

N/A 

1  ...255 

0  is  Reserved 

2 

3  ...  n 

Joint  1  -force  or 
torque 

int 

N  or  Nm 

If  revolute  joint 

Scaled  integer: 

Lower  limit  =  -1000  Nm 
Upper  limit  =  +1000  Nm 

If  prismatic  joint 

Scaled  integer: 

Lower  limit  =  -500  N 

Upper  limit  =  +500  N 

n  +  1 

Joint  n  -force  or 
torque 

int 

N  or  Nm 

see  field  2 

5.4  Sensor  Component  Applications  to  the  Puma  System 

Each  joint  onboard  the  Puma  762  manipulator  uses  an  incremental  encoder  for 
position  feedback  (Chapter  3,  Section  3.2.1).  This  information  is  obtained  from  the  Galil 
controller  in  encoder  counts  and  is  converted  to  radians.  Similarly,  the  velocity  is 
reported  in  encoder  counts  per  second  and  converted  into  radians  per  second  to  meet 
JAUS  specifications.  In  many  instances,  the  joint  data  is  converted  back  to  either  encoder 
counts  or  degrees  for  the  purposes  of  more  meaningful  interpretation. 

The  position  sensor  component  implemented  on  the  Puma  system  has  additional 
functionality  (Section  5.1.1).  This  involves  monitoring  joint  movement  and  assuring  that 
no  set  software  limits  have  been  breached.  It  further  uses  the  acquired  infonnation  to 
monitor  joint  angle  values  approaching  singularity  configurations  (Chapter  2,  Section 
2.3.4).  Actions  taken  if  either  occurs  are  discussed  in  Chapter  8.  The  use  of  MJPS  and 
MJVS  components  will  become  more  apparent  in  the  next  chapter  dealing  with 
closed-loop  control  of  joint  positions  and  velocities. 


CHAPTER  6 

MANIPULATOR  LOW-LEVEL  POSITION  AND  VELOCITY  DRIVER 

COMPONENTS 

These  components  take  as  inputs  the  desired  joint  positions  or  velocities,  or  the 
desired  end-effector  pose  or  velocity  state.  They  further  use  platform  specific  information 
provided  by  the  PM  component,  as  well  as  the  sensor  information  obtained  from  the 
MJPS  and  MJVS  components  to  perform  closed-loop  position  and  velocity  control.  It 
should  be  noted  that  in  most  implementations,  these  components  and  the  Primitive 
Manipulator  component  would  be  embedded  in  the  same  node  that  will  facilitate  the 
control  process.  The  implementation  of  the  low  level  drivers  to  the  Puma  platform 
becomes  nontrivial  and  is  addressed  in  Section  6.5. 

6.1  Manipulator  Joint  Positions  Driver  Component 

6.1.1  Component  Function 

The  function  of  the  Manipulator  Joint  Positions  Driver  (MJPD)  is  to  perform 
closed-loop  joint  position  control. 

6.1.2  Associated  Messages 

The  MJPD  (JAUS  ID#  54)  accepts  the  core  input  and  output  messages  (Chapter  1, 
Table  1-1).  User  defined  messages  that  are  received  or  sent  by  this  component  were 
defined  in  previous  chapters  and  are: 

•  Set  Joint  Positions 

•  Report  Manipulator  Specifications 

•  Report  Joint  Efforts 
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•  Report  Joint  Positions 

•  Set  Joint  Effort. 

6.1.3  Component  Description 

The  inputs  are  the  desired  joint  values,  current  joint  angles,  and  the  manipulator 
specifications  report.  The  output  is  the  joint  effort  level  that  is  sent  to  the  Primitive 
Manipulator  component.  Figure  6-1  shows  the  MJPD  component. 


Specifications  Message  Message 

actual  joint  effort ; 

Report  Joint  Effort  Message 


Figure  6-1.  Manipulator  Joint  Positions  Driver  component 

6.2  Manipulator  End-Effector  Pose  Driver  Component 

6.2.1  Component  Function 

The  function  of  the  Manipulator  End-Effector  Pose  Driver  (MEEPD)  is  to  perfonn 
closed-loop  position  and  orientation  control  of  the  end-effector. 

6.2.2  Associated  Messages 

The  MEEPD  (JAUS  ID#  55)  accepts  the  core  input  and  output  messages  (Chapter 
1,  Table  1-1).  Some  of  the  user  defined  messages  that  are  received  or  sent  by  this 
component  were  defined  in  previous  chapters,  while  others  are  defined  in  Section  6.2.4. 
The  following  messages  are  associated  with  this  component: 

• 


Set  Tool  Point 
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•  Set  End-Effector  Pose 

•  Query  Tool  Point 

•  Report  Manipulator  Specifications 

•  Report  Joint  Efforts 

•  Report  Joint  Positions 

•  Set  Joint  Effort 

•  Report  Tool  Point. 

6.2.3  Component  Description 

This  component  performs  closed-loop  position  and  orientation  control  of  the  end- 
effector.  The  input  is  the  desired  position  and  orientation  of  the  end-effector  specified  in 
the  vehicle  coordinate  system,  the  current  joint  angles,  and  the  data  from  the  manipulator 
specification  report.  The  output  is  the  joint  effort  level  that  is  sent  to  the  Primitive 
Manipulator  component.  The  component  is  shown  in  Figure  6-2. 


Report  Joint  Effort  Message 


Figure  6-2.  Manipulator  End-Effector  Pose  Driver  component 

6.2.4  Input  and  Output  Messages 
6.2.4.1  Code  0604h:  Set  Tool  Point  message 


This  message  specifies  the  coordinates  of  the  end-effector  tool  point  in  terms  of  the 
coordinate  system  attached  to  the  end-effector.  For  a  6-axis  robot,  this  coordinate  system 
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is  defined  by  having  its  origin  located  at  the  intersection  of  the  S6  joint  axis  vector  and 
the  user  defined  link  vector  a67.  The  z-axis  of  the  coordinate  system  is  along  S6  and  the  - 
x-axis  is  along  the  a67  vector. 


Table  6-1.  Set  Tool  Point  message  parameters 


Field  # 

Name 

Type 

Units 

Interpretation 

1 

x  coordinate  of  tool  point 

Int 

m 

Scaled  integer: 

Lower  limit  =  - 1 5  m 
Upper  limit  =  +15  m 

2 

y  coordinate  of  tool  point 

Int 

m 

See  field  1 

3 

z  coordinate  of  tool  point 

Int 

m 

See  field  1 

6.2.4.2  Code  0605h:  Set  End-Effector  Pose  message 

This  message  defines  the  desired  end-effector  position  and  orientation.  The 
coordinates  of  the  tool  point  are  defined  in  terms  of  the  vehicle  coordinate  system.  The 
orientation  of  the  end-effector  is  defined  by  a  unit  quaternion  (d  ;  a,  b,  c)  which  specifies 
the  axis  and  angle  of  rotation  that  was  used  to  establish  the  orientation  of  the  end-effector 
coordinate  system  with  respect  to  the  vehicle  coordinate  system. 


Table  6-2:  Set  End-Effector  Pose  message  parameters 


Field  # 

Name 

Type 

Units 

Interpretation 

1 

x  component  of  tool  point 

int 

m 

Scaled  integer: 

Lower  limit  =  -30m 
Upper  limit  =  +30m 

2 

y  component  of  tool  point 

int 

m 

see  field  1 

3 

z  component  of  tool  point 

int 

m 

see  field  1 

4 

d  component  of  unit  quaternion  q 

int 

N/A 

Scaled  integer: 

Lower  limit  =  -1 
Upper  limit  =  +1 

5 

a  component  of  unit  quaternion  q 

int 

N/A 

see  field  4 

6 

b  component  of  unit  quaternion  q 

int 

N/A 

see  field  4 

7 

c  component  of  unit  quaternion  q 

int 

N/A 

see  field  4 

6.2.4.3  Code  2604h:  Query  Tool  Point 

This  message  shall  cause  the  receiving  component  to  reply  to  the  requestor  with  a 
Code  4604h:  Report  Tool  Point  message. 
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6.2.4.4  Code  4604h:  Report  Tool  Point 

This  message  provides  the  receiver  with  the  current  values  of  the  joint  positions. 
The  message  data  for  the  Report  Joint  Positions  message  is  identical  to  Code  0604h:  Set 
Tool  Point. 

6.3  Manipulator  Joint  Velocities  Driver  Component 

6.3.1  Component  Function 

The  function  of  the  Manipulator  Joint  Velocities  Driver  (MJVD)  is  to  perfonn 
closed-loop  joint  velocity  control. 

6.3.2  Associated  Messages 

The  MJVD  (JAUS  ID#  56)  accepts  the  core  input  and  output  messages  (Chapter  1, 
Table  1-1).  User  defined  messages  that  are  received  or  sent  by  this  component  were 
defined  in  previous  chapters  and  are: 

•  Set  Joint  Velocities 

•  Report  Manipulator  Specifications 

•  Report  Joint  Effort 

•  Report  Joint  Velocities 

•  Set  Joint  Effort 

6.3.3  Component  Description 

The  input  consists  of  the  desired  instantaneous  joint  velocities,  the  current  joint 
velocities  (rad/s  for  revolute  and  m/s  for  prismatic  joints,  respectively),  and  the  data  from 
the  manipulator  specification  report  (Puma  platfonn  specific  parameters,  including  the 
orientation  of  the  manipulator  base  coordinate  system).  The  output  is  the  joint  effort 
level  that  is  sent  to  the  Primitive  Manipulator  component.  The  Manipulator  Joint 
Velocities  Driver  component  is  shown  in  Figure  6-3. 
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Specifications  Message 

actual  joint  effort ; 

Report  Joint  Effort  Message 


Figure  6-3.  Manipulator  Joint  Velocities  Driver  component 

6.4  Manipulator  End-Effector  Velocity  State  Driver  Component 

6.4.1  Component  Function 

The  function  of  the  Manipulator  End-Effector  Velocity  State  Driver  (MEEVD)  is  to 
perform  closed-loop  velocity  control  of  the  end  effector. 

6.4.2  Associated  Messages 

The  MEEPD  (JAUS  ID#  57)  accepts  the  core  input  and  output  messages  (Chapter 
1,  Table  1-1).  Some  of  the  user  defined  messages  that  are  received  or  sent  by  this 
component  were  defined  in  previous  chapters,  while  Set  End-Effector  Velocity  State 
message  is  defined  in  Section  6.4.4.  The  following  messages  are  associated  with  this 
component: 

•  Set  End-Effector  Velocity  State 

•  Report  Manipulator  Specifications 

•  Report  Joint  Effort 

•  Report  Joint  Positions 

•  Report  Joint  Velocities 


Set  Joint  Effort 
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6.4.3  Component  Description 

The  input  is  the  desired  end-effector  velocity  state,  specified  in  the  vehicle 
coordinate  system  or  the  end-effector  coordinate  system,  the  current  joint  positions  and 
joint  velocities,  and  the  data  from  the  manipulator  specifications  report.  The  output  is  the 
joint  effort  level  that  is  sent  to  the  Primitive  Manipulator  component.  Figure  6-4  depicts 
this  component. 


commanded  end-effector  velocity 


Specifications  Message  actual  joint  effort ; 

Report  Joint  Effort  Message 

Figure  6-4.  Manipulator  End-Effector  Velocity  State  Driver  component 
6.4.4  Code  0606h:  Set  End-Effector  Velocity  State  message 

The  velocity  state  of  body  B  measured  with  respect  to  body  A  is  defined  as  the 


angular  velocity  of  body  B  with  respect  to  body  A,  AcaB,  and  the  linear  velocity  of  the 


point  in  body  B  that  is  coincident  with  the  origin  of  the  reference  frame  measured  with 
respect  to  body  A,  A  v®  .  From  these  parameters,  the  velocity  of  any  point  in  body  B  can 


be  determined  from  the  equation  A  vp =A  v0B +AcaB  x  r0^p  .  Here  A  vP  is  the  velocity  of 


some  point  P,  that  is  embedded  in  body  B,  measured  with  respect  to  body  A  and  r0^p 


represents  the  vector  from  the  origin  of  the  reference  frame  to  point  P,  i.e.  the  coordinates 
of  point  P.  In  this  application,  body  B  is  the  end-effector,  and  body  A  is  ground.  The 
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reference  coordinate  system  is  embedded  in  ground,  but  is  aligned  with  either  the  end- 
effector  coordinate  system  or  the  vehicle  coordinate  system  at  this  instant. 


Table  6-3.  Set  End-Effector  Velocity  State  message  parameters 


Field  # 

Name 

Type 

Units 

Interpretation 

1 

Coordinate  system 
definition 

byte 

N/A 

1  =  reference  coord, 
system  aligned  with 
vehicle  coord,  sys. 

2  =  reference  coord, 
system  aligned  with 
end-effector  coord,  sys. 

2 

x  component  of  angular 
velocity  AcoB 

int 

rad/sec 

Scaled  integer: 

Lower  limit  =  -20  n 
rad/sec 

Upper  limit  =  +20  n 
rad/sec 

3 

y  component  of  angular 
velocity  AwB 

int 

rad/sec 

see  field  2 

4 

z  component  of  angular 
velocity  AcoB 

int 

rad/sec 

see  field  2 

5 

x  component  of  the  linear 
velocity  Av® 

int 

m/sec 

Scaled  integer: 

Lower  limit  =  -10 
m/sec 

Upper  limit  =  +10 
rad/sec 

6 

y  component  of  the  linear 
velocity  Av® 

int 

m/sec 

see  field  5 

7 

z  component  of  the  linear 
velocity  A  v® 

int 

m/sec 

see  field  5 

6.5  Applications  of  the  Low-Level  Driver  Components  to  the  Puma  System 

The  complexity  of  low-level  component  implementation  revolves  around  the 
concept  of  joint  effort.  Even  though  the  Galil  controller  is  inherently  capable  of  position 
and  velocity  control,  the  interpretation  of  effort  as  velocity  mandates  the  use  of 
independent  jogging  mode  to  perpetuate  joint  motion  (Chapter  3,  Section  3.3.1). 
Therefore  this  section  strives  to  relate  this  interpretation  to  each  individual  component’s 
requirements. 


59 


determined  experimentally  for  each  of  the  joints  and  depend  on  the  chosen  values  of 
maximum  speed,  acceleration  and  deceleration.  Since  joint  efforts  can  vary  from  -100% 
to  100%,  computed  efforts  are  clipped  when  received  by  the  Primitive  Manipulator.  For 
instance,  if  commanded  joint  position  is  -3000  encoder  counts,  and  the  current  value  is 
6000  encoder  counts,  then  the  PM  will  cause  that  joint  to  move  at  a  100%  of  the 
maximum  velocity  in  the  negative  direction.  As  the  position  gap  closes,  the  difference 
becomes  smaller  causing  the  effort  to  decrease.  When  the  desired  position  is  reached,  the 
resulting  effort  is  zero.  This  implementation  thus  adds  another  control  loop  to  the  system. 
Since  the  independent  jogging  mode  uses  trajectory  generator,  there  is  a  possibility  of 
overshoot.  Equation  6-1  is  set  up  to  quickly  fix  the  error  and  return  the  joint  back  to  the 
desired  position. 

The  Manipulator  End-Effector  Pose  Driver  component  receives  the  desired  position 
and  orientation  of  the  end-effector  as  its  inputs.  It  then  uses  the  reverse  position  analysis 
(Section  2.3.2)  to  compute  the  corresponding  joint  values.  Just  as  with  the  MJPD,  this 
component  constantly  queries  for  current  joint  positions.  Once  this  information  is 
available,  the  identical  approach  (Equation  6-1)  is  used  to  complete  the  task. 
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Definition  of  the  joint  effort  for  the  two  components  commanding  velocity  is  much 
simpler  and  is  given  by  Equation  6-2. 

efforts-  (6-2) 

velocity  [n] 

The  Manipulator  Joint  Velocities  Driver  component  receives  the  desired  joint 
velocities,  while  the  MEEVD  uses  reverse  velocity  analysis  from  Section  2.3.3  to 
determine  these  values  given  the  desired  velocity  state  of  the  end-effector.  Once 
computed,  these  values  are  passed  on  to  the  Primitive  Manipulator.  The  effort  values 
should  never  exceed  -100%  or  100%  (e.g,  commanding  a  -2500  encoder  count  per  second 
velocity  would  be  converted  to  -50%,  given  the  maximum  velocity  of  5000  encoder 
counts  per  second).  It  is  entirely  up  to  the  Galil  controller  to  close  the  loop  and  ensure 
joint  motion  at  the  desired  speed. 

This  chapter  explored  all  the  core  functionality  of  the  JAUS  manipulator 
implementation.  The  following  chapter  further  takes  the  above  capabilities  and  allows  the 
system  to  handle  multiple  sets  of  joint  positions  and  end-effector  positions  and 


orientations. 


CHAPTER  7 

MID-LEVEL  POSITION  AND  VELOCITY  DRIVER  COMPONENTS 
This  chapter  defines  the  last  2  components  with  respect  to  the  scope  of  our  study. 
Up  to  this  point,  this  modular  architecture  was  only  concerned  with  single-goal 
completion.  The  components  presented  in  this  chapter  will  extend  this  interface  to  allow 
the  manipulator  to  receive  multiple  joint  position  configurations  or  a  tool-point  path 
profile.  It  should  be  noted  that  in  most  implementations,  these  components  and  the 
Primitive  Manipulator  will  be  embedded  in  the  same  node  which  will  facilitate  the 
control  process.  Finally,  the  last  section  will  briefly  discuss  how  these  2  components  are 
implemented  on  the  Puma  platform. 

7.1  Manipulator  Joint  Move  Driver  Component 

7.1.1  Component  Function 

The  function  of  the  Manipulator  Joint  Move  Driver  (MJMD)  is  to  perform  closed- 
loop  joint  level  control  of  the  manipulator  where  motion  parameters  for  each  joint  are 
specified.  The  specified  motion  parameters  are  the  desired  values,  maximum  velocity, 
maximum  acceleration,  and  maximum  deceleration. 

7.1.2  Associated  Messages 

The  MJMD  (JAUS  ID#  58)  accepts  the  core  input  and  output  messages  (Chapter  1, 
Table  1-1).  Some  of  the  user  defined  messages  that  are  received  or  sent  by  this 
component  were  defined  in  previous  chapters,  while  Set  Joint  Motion  message  is  defined 
in  Section  7. 1 .4.  The  following  messages  are  associated  with  this  component: 

•  Set  Joint  Motion 
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•  Report  Manipulator  Specifications 

•  Report  Joint  Effort 

•  Report  Joint  Positions 

•  Report  Joint  Velocities 

•  Set  Joint  Effort 

7.1.3  Component  Description 

The  inputs  are  the  desired  joint  values  at  specified  time  values  together  with  data  to 
define  a  trapezoidal  velocity  profile  (i.e.  the  maximum  joint  velocity,  maximum  joint 
acceleration,  and  maximum  joint  deceleration).  No  explicit  path  is  defined,  only  the 
values  of  the  joint  angles  at  distinct  times.  The  time  values  are  measured  in  units  of 
seconds  and  are  relative  to  the  time  that  the  movement  to  the  first  joint  angle  set  is 
started.  Additional  inputs  are  the  current  joint  values,  joint  velocities,  and  the  data  from 
the  Report  Manipulator  Specifications  message.  The  output  is  the  joint  effort  level  that  is 
sent  to  the  Primitive  Manipulator  component.  The  MJMD  is  shown  in  Figure  7-1. 


commanded  joint  motion  profile  ; 
Set  Joint  Motion  Message 


actual  joint  effort ; 

Report  Joint  Effort  Message 


Figure  7-1.  Manipulator  Joint  Move  Driver  component 
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7.1.4  Code  0607:  Set  Joint  Motion 

Note  that  this  message  specifies  the  number  of  manipulator  joints  and  the  number 
of  poses  (different  trapezoidal  profile  sets). 


Table  7-1.  Set  Joint  Motion  message  parameters 


Field  # 

Name 

Type 

Units 

Interpretation 

1 

Num  Joints,  n 

Byte 

N/A 

1  ...255 

0  is  Reserved 

2 

number  of 
poses,  p 

Byte 

N/A 

1...255 

0  is  Reserved 

3 

pose  1  time 

int 

Sec 

Scaled  integer: 

Lower  limit  =  0  sec 

Upper  limit  =  6000  sec 

4 

Joint  1  - 
position  at 
pose  1 

int 

rad  or  m 

Scaled  integer: 

If  revolute  joint, 

Lower  limit  =  -8tt  rad 

Upper  limit  =  +871  rad 

If  prismatic  joint, 

Lower  limit  =  -10  m 

Upper  limit  =  +10  m 

5 

Joint  1  -  max 
velocity 

int 

rad/s  or  m/s 

If  revolute  joint, 

Lower  limit  =  -IO71  rad/s 
Upper  limit  =  +10tt  rad/s 

If  prismatic  joint, 

Lower  limit  =  -5  m/s 

Upper  limit  =  +5  m/s 

6 

Joint  1  -  max 
acceleration 

int 

rad/s2  or  m/s2 

If  revolute  joint, 

2 

Lower  limit  =  -IO71  rad/s 
Upper  limit  =  +10tt  rad/s" 

If  prismatic  joint, 

Lower  limit  =  -20  m/s" 
Upper  limit  =  +20  m/s2 

7 

Joint  1  -  max 
deceleration 

int 

rad/s2  or  m/s2 

If  revolute  joint, 

2 

Lower  limit  =  -1  Om  rad/s 
Upper  limit  =  +10tt  rad/s“ 

If  prismatic  joint, 

Lower  limit  =  -20  m/s2 
Upper  limit  =  +20  m/s2 

8 

Joint  2  - 
position  at 
pose  1 

int 

rad  or  m 

see  field  4 

9 

Joint  2  -  max 
velocity 

int 

rad/s  or  m/s 

see  field  5 
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Table  7-1.  Continued 


Field  # 

Name 

Type 

Units 

Interpretation 

10 

Joint  2  -  max 
acceleration 

int 

2  2 

rad/s  or  m/s' 

see  field  6 

11 

Joint  2  -  max 
deceleration 

int 

2  2 

rad/s  or  m/s' 

see  field  7 

4n 

Joint  n  - 
position  at 
pose  1 

int 

rad  or  m 

see  field  4 

4n+l 

Joint  n  -  max 
velocity 

int 

rad/s  or  m/s 

see  field  5 

4n+2 

Joint  n  -  max 
acceleration 

int 

rad/s2  or  m/s2 

see  field  6 

4n+3 

Joint  n  -  max 
deceleration 

int 

rad/s  or  m/s' 

see  field  7 

(p- 1 )  4n+4 

pose  p  time 

int 

s 

see  field  3 

(p- 1 )  4n+5 

Joint  1  - 
position  at 
pose  p 

int 

rad  or  m 

see  field  4 

(p- 1 )  4n+6 

Joint  1  -  max 
velocity 

int 

rad/s  or  m/s 

see  field  5 

(p- 1 )  4n+7 

Joint  1  -  max 
acceleration 

int 

rad/s2  or  m/s2 

see  field  6 

(p- 1 )  4n+8 

Joint  1  -  max 
deceleration 

int 

rad/s2  or  m/s2 

see  field  7 

(P- 1 )8n 

Joint  n  - 
position  at 
pose  p 

int 

rad  or  m 

see  field  4 

(p-l)8n+l 

Joint  n  -  max 
velocity 

int 

rad/s  or  m/s 

see  field  5 

(p-l)8n+2 

Joint  n  -  max 
acceleration 

int 

rad/s2  or  m/s2 

see  field  6 

(p-l)8n+3 

Joint  n  -  max 
deceleration 

int 

rad/s  or  m/s' 

see  field  7 
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7.2  Manipulator  End-Effector  Discrete  Pose  Driver  Component 

7.2.1  Component  Function 

The  function  of  the  Manipulator  End-Effector  Discrete  Pose  Driver  (MEEDPD)  is 
to  perform  closed-loop  control  of  the  end-effector  pose  through  a  series  of  specified 
positions  and  orientations. 

7.2.2  Associated  Messages 

The  MEEDPD  (JAUS  ID#  59)  accepts  the  core  input  and  output  messages  (Chapter 
1,  Table  1-1).  Some  of  the  user  defined  messages  that  are  received  or  sent  by  this 
component  were  defined  in  previous  chapters,  while  Set  End-Effector  Path  Motion 
message  is  defined  in  Section  6.4.4.  The  following  is  the  list  of  all  the  messages 
associated  with  this  component: 

•  Set  End-Effector  Path  Motion 

•  Report  Manipulator  Specifications 

•  Report  Joint  Effort 

•  Report  Joint  Positions 

•  Report  Joint  Velocities 

•  Set  Joint  Effort 

7.2.3  Component  Description 

This  component  performs  closed-loop  control  of  the  end-effector  pose  as  measured 
with  respect  to  the  vehicle  coordinate  system.  The  inputs  are  a  path  motion  description 
(discrete  end-effector  position  and  orientation  at  time  t  measured  in  the  vehicle 
coordinate  system  which  is  defined  by  a  point  and  a  quaternion  at  time  t),  the  current 
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joint  values,  the  current  joint  velocities,  and  the  data  from  the  Report  Manipulator 


Specifications  message.  The  output  is  the  joint  effort  level  that  is  sent  to  the  Primitive 


Manipulator  component.  The  functionality  of  the  MEEDPD  is  shown  in  Figure  7-2. 


commanded  end- 
effector  path  motion 
profile  in  vehicle 
coordinate  system ; 
Set  End-effector  Path 


Report  Manipulator  actual  joint  effort ; 
Specifications  Message  Report  Joint  Effort 
Message 


Figure  7-2.  Manipulator  End-Effector  Discrete  Pose  Driver  component 
7.2.4  Code  0608h:  Set  End-Effector  Path  Motion 

A  series  of  end-effector  poses  are  defined  in  terms  of  the  vehicle  coordinate  system 
at  various  times.  The  time  is  a  relative  time  and  is  defined  in  seconds  where  time  equals 
0  is  the  moment  that  the  motion  towards  the  first  pose  begins. 

7.3  Applications  of  the  Mid-Level  Driver  Components  to  the  Puma  System 
The  Section  3.3.1  described  the  independent  axis  positioning  mode  inherent  to  the 
Galil  controller.  The  parameters  defining  joint  motion  in  this  mode  are  identical  to  those 
defined  by  Set  Joint  Motion  message.  Furthermore,  the  independent  jogging  mode  used 
in  previous  component  definitions  provides  closed-loop  position  control  generating  a 
trapezoidal  trajectory  based  on  these  parameters.  Thus,  the  MJMD  can  be  defined  as  a 
superset  of  the  MJPD  component,  in  that  it  redefines  the  maximum  speed,  acceleration 
and  deceleration  with  each  new  pose  instead  of  using  the  default  values. 
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Table  1-2.  Set  End -Effector  Path  Motion  message  parameters 


Field  # 

Name 

Type 

Units 

Interpretation 

1 

number  of  poses,  n 

Byte 

N/A 

1  ...255 

0  is  Reserved 

2 

time  1 

int 

sec 

time  for  pose  1 

Scaled  integer: 

Lower  limit  =  0  sec 

Upper  limit  =  6000  sec 

3 

X  component  of  tool 
point  for  pose  1 

int 

m 

Scaled  integer: 

Lower  limit  =  -30  m 

Upper  limit  =  +30  m 

4 

Y  component  of  tool 
point  for  pose  1 

int 

m 

see  field  3 

5 

Z  component  of  tool  point 
for  pose  1 

int 

m 

see  field  3 

6 

d  component  of  unit 
quaternion  q  for  pose  1 

int 

N/A 

Scaled  integer: 

Lower  limit  =  -1 

Upper  limit  =  +1 

7 

a  component  of  unit 
quaternion  q  for  pose  1 

int 

N/A 

see  field  6 

8 

b  component  of  unit 
quaternion  q  for  pose  1 

int 

N/A 

see  field  6 

9 

c  component  of  unit 
quaternion  q  for  pose  1 

int 

N/A 

see  field  6 

8n-6 

time  n 

int 

sec 

see  field  2 

8n-5 

X  component  of  tool 
point  for  pose  n 

int 

m 

see  field  3 

8n-4 

Y  component  of  tool 
point  for  pose  n 

int 

m 

see  field  3 

8n-3 

Z  component  of  tool  point 
for  pose  n 

int 

m 

see  field  3 

8n-2 

d  component  of  unit 
quaternion  q  for  pose  n 

int 

N/A 

see  field  6 

8n-l 

a  component  of  unit 
quaternion  q  for  pose  n 

int 

N/A 

see  field  6 

8n 

b  component  of  unit 
quaternion  q  for  pose  n 

int 

N/A 

see  field  6 

8n+l 

c  component  of  unit 
quaternion  q  for  pose  n 

int 

N/A 

see  field  6 

An  identical  approach  is  taken  to  determine  the  corresponding  joint  efforts 
(Equation  6-1).  Note  however,  that  the  proportional  constant  kp  should  change  with  the 
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magnitude  of  these  parameters.  Since  this  implementation  is  used  to  test  rather  then 
optimize  manipulator  performance,  previously  determined  values  of  k p  are  adequate  for 

operating  speeds  below  10000  encoder  counts  per  second.  Thus  after  each  pose  is 
completed  a  new  set  of  parameters  is  applied  to  the  system. 

Unlike  the  MJMD,  the  functionality  of  the  MEEDPD  component  is  identical  to  that 
of  the  MEEPD.  It  just  allows  for  a  single  message  to  define  multiple  tool-point  positions 
and  orientations  to  be  executed  autonomously  at  a  particular  time.  Note  that  this 
implementation  uses  generic  timing  functions  provided  with  the  Linux  kernel.  More 
precise  operation  would  require  the  use  of  a  real-time  operating  system.  All  aspects  of 
software  design  are  addressed  in  the  following  chapter. 


CHAPTER  8 

OVERVIEW  OF  SOFTWARE  DESIGN 
The  purpose  of  our  study  was  to  design  and  implement  the  JAUS  manipulator 
components.  The  design  aspects  were  covered  in  Chapters  4,  5,  6,  and  7,  while  the 
software  implementation  is  addressed  here.  Keep  in  mind  that  the  chapter  covers  design 
and  logistics  aspects  rather  than  computer  science  technicalities.  The  source  code  is 
rather  complicated  but  well  documented  and  could  be  easily  read.  For  completion 
purposes,  this  chapter  gives  a  brief  overview  of  the  node  manager  and  its  role  in 
inter-component  communications. 

8.1  Overview 

The  scope  of  computer  programming  associated  with  each  of  the  components  and 
their  corresponding  messages  reflects  the  importance  of  software  design  in  this  and  any 
other  large  scale  project.  In  order  to  allow  for  easy  and  clear  understanding  behind  the 
process  control  and  data  flow  in  each  of  the  functional  elements,  this  chapter  begins  its 
analysis  at  the  lowest  level  and  moves  up  hierarchically.  More  specifically,  it  first 
addresses  the  logistics  behind  the  interface  to  the  Galil  controller,  then  goes  into  the 
component  and  message  development,  and  finally  describes  the  node  level  functionality. 
Due  to  a  high  level  of  redundancy,  only  one  component  out  of  the  first  three  groups  will 
be  analyzed  in  detail.  Furthermore,  this  development  takes  advantage  of  multithreading,  a 
powerful  tool  described  in  many  popular  computer  science  books.  From  a  software 
development  standpoint,  each  component  is  treated  as  a  separate  thread,  and  could  be 
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compared  to  a  program  running  a  while-loop.  An  additional  thread  is  used  in  the  interface 


to  the  controller. 


8.2  The  Interface  to  the  Galil  Controller 

The  Section  3.4  gave  an  overview  of  the  C  function  prototypes  that  are  the  core  of 
this  development.  Only  a  single  program/thread  is  responsible  for  communications  with 
the  Galil  controller  eliminating  the  need  for  multithread  synchronization.  The  file 
galillnterface.c  is  responsible  for  managing  startup  and  shutdown  routines  of  this 
particular  thread.  The  interface  has  a  well  defined  structure  that  is  shown  in  Figure  8-1 : 


Figure  8-1.  The  galillnterface.c  logic  flow  diagram 
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On  startup,  the  code  checks  whether  the  RoboWorks  3-D  software  tool  is  used. 

This  package,  developed  at  the  University  of  Texas,  was  slightly  modified  to  work  with 
the  Linux  operating  system  for  both  simulation  and  real-time  3D  display  purposes.  Its 
application  will  become  more  apparent  in  the  next  chapter.  RoboTalk  is  the  program  that 
handles  Ethernet  communications  between  the  computer  hosting  RoboWorks  and  the 
computer  providing  position  data.  The  file  RoboTalk.h  contains  function  prototypes  for 
the  Connect(),  SetTagValues(..),  and  Disconnect()  functions  shown  in  Figure  8-1.  This  is 
followed  by  establishing  a  connection  to  the  Galil  controller,  resetting  it  for  safety 
purposes,  and  calibrating  all  the  joints  using  stored  potentiometer  values. 

After  the  state  thread  is  started,  position,  velocity  and  force/torque  values  are 
queried  using  “TP”,  “TV”,  and  “TT”  commands,  respectively.  Regardless  of  the  fact  that 
the  Puma  762  does  not  have  any  torque  sensors,  this  component  was  implemented  for 
testing  purposes.  Commanding  an  independent  jogging  motion  is  accomplished  using  the 
“JG”  command  (e.g.  if  the  Primitive  Manipulator  commands  a  50%  effort,  this  is 
converted  to  -2500  encoder  counts  per  second  velocity  using  Equation  6-2,  assuming  the 
maximum  velocity  is  5000  encoder  counts  per  second).  In  addition  to  the  core 
functionality,  galillnterface.c  is  used  to  set  up  maximum  speed,  acceleration,  and 
deceleration  values  using  “SP”,  “AC”,  and  “DC”  commands  generating  a  trapezoidal 
profile.  The  function  is  set  as  a  sleeper  outside  the  thread  and  will  change  the  values  only 
if  called.  Only  the  Manipulator  Joint  Motion  component  is  responsible  for  changes  to  the 
default  parameters.  The  file  also  contains  a  homing  function  that  could  take  the 
manipulator  to  its  home  position  on  startup.  This  capability  should  be  used  after 
recalibration  of  joint  values.  The  source  code  for  galillnterface.c,  RoboTalk.c,  and 
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corresponding  header  files  is  listed  in  Appendix  B,  Section  B.l.  The  Primitive 
Manipulator  component  and  all  of  the  sensor  components  acquire  data  from  the  interface 
file  using  external  access  functions  eliminating  the  need  for  the  use  of  global  variables. 

8.3  Component  Level  Software  Development 
Components  are  based  on  standardized  interfaces  (Section  1.3.4).  From  a  software 
development  standpoint,  this  means  that  the  code  structure  of  the  files  defining  these 
unique  JAUS  entities  is  very  similar.  Figure  8-2  shows  the  functional  elements  of  a 
generic  component  file. 


cmptc 


Component  startup 


_ i _ 

Component  check-in 
with  the  node 
manager  to  obtain 
Instance,  Node  and 
Subsystem  IDs 


_ i _ 

Open  connection  with 
the  node  manager 
and  obtain  jms 
accessor 


1 

Set  component  state 
to  INITIALIZE 


_ t _ 

Start  component 
state  thread 


_ 

Start  component 
node  manager  thread 


I 


Figure  8-2.  Generic  component  logic  flow  diagram 
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Each  component  follows  a  standard  process  flow  defining  thread  startup  and 
shutdown  routines.  As  it  is  shown  in  Figure  8-2,  each  component  consists  of  two  threads. 
The  first  is  responsible  for  perfonning  component  function  while  the  other  handles  the 
incoming  core  and  user-defined  messages.  Therefore  it  is  in  the  state  thread  that  the 
particular  component’s  behavior  is  defined.  This  implementation  defines  Emergency, 
Failure,  and  Shutdown  states  identically  across  the  entire  node.  In  order  to  better 
understand  the  inner  workings  of  these  JAUS  elements,  the  state  thread  of  one  component 
from  each  of  the  Chapters  4,  5,  and  6  is  analyzed. 

8.3.1  Primitive  Manipulator  State  Thread 

The  best  way  to  represent  the  processes  within  a  particular  file  is  using  a  flow 
chart.  Therefore,  Figure  8-3  shows  the  PM  state  thread. 


Figure  8-3.  The  Primitive  Manipulator  state  thread  logic  flow  diagram 
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Each  component  is  set  to  the  Initialize  state  by  default  during  the  startup  routine. 
While  at  this  state,  the  PM  accesses  the  information  provided  by  the  interface  to  the  Galil 
controller  requesting  the  status  on  initialization  procedures,  ensuring  all  integrity  checks 
are  complete  and  the  system  is  up  and  running.  Once  the  component  enters  the  Ready 
state,  it  talks  to  the  interface  file  again,  but  this  time  accessing  the  function  directly 
responsible  for  commanding  motion,  given  the  joint  effort  values.  The  source  code  for 
the  file  pm.c  and  its  corresponding  header  is  documented  in  Appendix  B.2. 

8.3.2  Manipulator  Sensor  Components 

The  state  thread  of  Manipulator  Joint  Position  Sensor  is  described  in  this  section  as 
it  provides  more  complex  functionality  than  the  other  sensor  components.  Definitions  of 
Initialize,  Standby  and  Ready  states  are  shown  in  Figure  8-4. 


Figure  8-4.  Manipulator  Joint  Position  Sensor  logic  flow  diagram 
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The  definition  of  the  initialize  state  in  the  above  diagram  is  applied  to  the 
remainder  of  the  components  on  this  node.  If  the  query  status  message  to  the  Primitive 
Manipulator  returns  Ready,  it  is  assumed  that  the  initialization  procedure  was  successful. 
Once  in  the  Ready  state,  the  MJPS  accesses  the  Galil  interface  tile  and  queries  it  for 
position.  Next,  it  checks  those  values  ensuring  that  none  of  the  joints  are  crossing 
position  limits,  or  getting  close  to  a  singularity  configuration.  In  either  case,  the  MJPS 
changes  the  status  of  the  Primitive  Manipulator  to  the  Emergency  state.  This  instantly 
causes  all  the  other  components  to  go  into  the  Emergency  state  as  well.  The  system 
would  now  have  to  be  reinitialized.  The  source  code  for  the  mjps.c  file  and  the 
corresponding  header  is  listed  in  the  Appendix  B.3. 

8.3.3  Low-Level  Position  and  Velocity  Drivers 

It  should  be  noted,  that  all  states,  except  Ready,  are  defined  in  the  same  manner  as 
covered  in  previous  sections  across  all  of  the  remaining  components.  Figure  8-5  shows 
the  MEEPD  state  thread  (Ready  state  only). 


Figure  8-5.  Manipulator  End-Effector  Pose  Driver  logic  flow  diagram 
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The  Figure  8-5  simply  states  that  this  component  performs  its  unique  function  as 
long  as  there  is  no  change  in  the  status  of  the  Primitive  Manipulator.  At  this  point  all  of 
the  software  functionality  is  defined,  and  since  the  timing  functionality  of  the  mid-level 
components  is  a  trivial  addition,  it  is  not  discussed  in  detail.  However,  for  completion 
purposes,  Appendix  B.4  and  Appendix  B.5  list  the  meepd.c  and  the  meedpd.c  files  and 
their  corresponding  headers,  respectively. 

8.4  Message  Level  Software  Development 
Each  manipulator  component  sends  or  receives  user  defined  messages.  Prior  to 
messages  being  exchanged,  they  are  compressed  (packed)  and  eventually  uncompressed 
(unpacked)  once  they  reach  the  destination  (Section  1.3.5).  Packing  and  unpacking 
algorithms  are  well  defined  and  easy  to  implement.  They  are  based  on  converting  a 
desired  parameter  into  a  byte  stream,  thus  using  a  correct  byte  counter  is  critical  for 
preserving  correct  values.  These  functions  can  further  clip  passing  parameters  if  they  lay 
outside  the  specified  limits.  Information  being  passed  is  defined  as  a  structure  type  so  that 
it  is  easily  accessed  by  the  components  requesting  it.  For  example,  joint  velocities  are 
sent  by  the  MJVD  component  as  a  part  of  a  jointVelocity  t  structure  containing  six 
velocity  values  and  a  parameter  defining  the  total  number  of  joints.  Message  files  and 
their  corresponding  headers  are  documented  in  the  Appendix  C. 

8.5  Node  Level  Software  Development 
The  Manipulator  Control  node  is  responsible  for  startup  and  shutdown  of  all  the 
manipulator  components.  In  the  case  of  the  Puma  system,  it  also  ensures  that  both  the  Val 
and  Galil  controllers,  as  well  as  the  ann  itself  are  powered  up  before  starting  the  interface 
thread.  The  order  in  which  the  components  are  started  up  is  irrelevant,  but  should  be 
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consistent  with  the  order  in  which  they  are  shutdown.  Two  files,  mc.c  and  mc.h,  are  listed 
in  the  Appendix  B.6. 

8.6  Node  Manager  and  Communicator 

Two  JAUS  components  responsible  for  all  inter-component  communications  are 
the  Communicator  and  the  Node  Manger.  The  Communicator  allows  for  a  single  point  of 
message  entry  into  a  subsystem  while  maintaining  the  data  link  between  the  subsystems. 
The  Node  Manger  component  is  responsible  for  routing  JAUS  messages  from  one  node 
to  another.  When  a  component  sends  a  message  [7],  it  first  goes  to  the  Node  Manager 
where  the  information  regarding  the  destination  component  is  interpreted  and  passed  onto 
the  communicator,  which  handles  the  transmission  of  the  message  to  the  proper  nod. 

Finally,  at  the  highest  level,  the  main  file  is  responsible  for  starting  the  node  itself. 
This  file  further  uses  a  “curses”  text-based  window  environment  for  display  purposes. 
How  the  information  is  being  displayed  for  each  of  the  components  is  discussed  in  the 
next  chapter.  The  main.c  file  is  listed  in  the  Appendix  B.7. 


CHAPTER  9 

TESTING  AND  RESULTS 

This  chapter  covers  the  methods  used  to  test  2  aspects  of  this  JAUS 
implementation.  The  first  ensures  that  the  functionality  of  developed  messages  and 
components  meets  the  JAUS  specification  requirements.  The  latter  looks  at  how  well  this 
architecture  handles  manipulator  control  from  the  view  point  of  accuracy  and  task 
completion.  In  order  to  adequately  test  this  implementation,  an  additional  component 
mimicking  a  Subsystem  Commander  (SSC)  was  created.  The  manipulator  sensor 
components  are  self  tested  as  the  information  they  provide  becomes  critical  to 
components  responsible  for  closed-loop  position  and  velocity  control.  Essentially  7 
different  cases  or  scenarios  pertaining  to  the  functionality  of  the  driver  components  are 
evaluated  by  the  SSC.  User  input  is  required  when  choosing  the  scenario,  however  the 
current  setup  mandates  that  any  parameter  change  has  to  be  done  in  the  source  code. 

9.1  Subsystem  Commander  Component  Overview 
The  Subsystem  Commander  component  is  defined  in  the  JAUS  Reference 
Architecture  document.  Its  function  is  to  coordinate  all  activity  within  a  given  subsystem. 
The  SSC  (JAUS  ID#  32)  has  the  responsibility  of  performing  mission  planning,  issuing 
commands,  and  querying  status  for  the  subsystem  operation.  The  document  states  that 
functions  of  this  component  may  be  performed  by  humans,  or  by  the  computer  or  both 
[7].  The  capabilities  of  the  RoboWorks  software  package  are  integrated  with  this 
component  to  display  real-time  position  data  of  the  manipulator.  This  could  be  useful 
when  the  commander  is  located  off  site.  Figure  9-1  shows  this  operator  control  setup. 
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Figure  9-1.  Subsystem  Commander  operator  graphical  user  interface 

9.2  Case  1:  Set  Joint  Effort 

The  first  scenario  is  used  to  test  the  Primitive  Manipulator  component  without  any 
sensor  feedback.  In  order  to  ensure  that  no  other  components  are  commanding  joint 
efforts,  their  status  is  changed  to  the  Standby  state.  Note  that  the  case  numbers 
correspond  to  the  items  listed  in  Figure  9-1.  Set  Joint  Effort  Message  commands  the 
following  set  of  values: 

effort  =  [  15  -15  15  -12  12  -12]  (9-1) 

Given  the  default  maximum  velocity  value  of  5000  encoder  counts  per  second 
(enc/s)  for  the  first  three  joints  and  1000  enc/s  for  the  last  three  joints,  these  values  are 
converted  back  to  [750,  -750,  750,  -120,  120,  -120]  enc/s  as  they  are  passed  on  to  the 
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Galil  controller  using  Equation  6-2.  Figure  9-2  shows  a  snapshot  of  the  Primitive 


Manipulator  screen  during  the  execution  of  this  command. 


Primitiue  Manipulator 

Screen 

Commanded 

Effort 

PM  State: 

Ready 

Joint  1 : 

15.00 

PM  Instance  ID: 

1 

Joint  2: 

-15.00 

PM  Component  ID: 

49 

Joint  3: 

15.00 

PM  Node  ID: 

1 

Joint  4: 

-12.00 

PM  Subsystem  ID: 

200 

Joint  5: 

12.00 

PM  Update  Rate: 

331 .67 

Joint  6: 

-12.00 

Interface  Update  Rate: 

7.45 

Manipulator  Specifications 

Current 

Effort 

Tube  Lnk  L  Twist  Anq 

Offset 

Pos  Limit 

Uel  Limit 

Joint  1 : 

15.20 

(mm) 

(deg) 

(mm) 

(enc) 

(enc/s) 

Joint  2: 

-15.00 

Joint  3: 

15.18 

Joint  1:1  0 

90.0 

0 

224000 

5000 

Joint  4: 

-11 .50 

Joint  2:  1  650 

0.0 
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Figure  9-2.  Primitive  Manipulator  Screen  as  it  responds  to  the  Set  Joint  Effort  message 


Note  that  the  current  effort  values  are  slightly  different  from  the  commanded  ones. 
This  error  ranging  between  1%  and  4%  is  inherent  to  the  underlying  theory  behind  the 


independent  jogging  mode  covered  in  Chapter  3.  Figure  9-2  also  shows  that  the  PM 


screen  is  used  to  display  the  contents  of  the  Manipulator  Specifications  message. 


Mechanism  and  controller  information  is  also  available.  It  is  useful  to  realize  that  in  this 


and  the  upcoming  sections,  any  information  regarding  position  and  velocity  information 
is  displayed  in  units  of  encoder  counts  (enc)  and  encoder  counts  per  second  (enc/s), 
respectively.  This  was  done  for  practical  purposes  to  allow  for  quick  interpretation  of  the 
corresponding  positions  with  the  actual  ones.  Table  1-1  lists  the  conversion  factors  used 
to  go  from  specified  JAUS  standard  units  to  enc  and  enc/s  and  vice  versa: 
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Table  9-1.  The  Puma  762  platform  specific  conversion  factors 


Joint  # 

Radians  to  Encoder  Counts 

Encoder  Counts  to  Degrees 

1 

45836.6 

0.00125 

2 

65951.9 

0.00086875 

3 

51357.5 

0.001115625 

4 

9152.6 

0.00626 

5 

9152.6 

0.00626 

6 

4285.3 

0.01337 

9.3  Case  2:  Set  Joint  Position 

This  case  tests  the  capabilities  of  the  Manipulator  Joint  Position  Driver.  The  SSC 
simply  commands  six  joint  values  corresponding  to  the  desired  joint  positions.  That 
information  is  then  used  to  determine  the  corresponding  joint  efforts  (Equation  6-1)  that 
are  passed  to  the  PM.  Unlike  the  PM,  MJVD,  and  MEEVD  components,  this  and  the 
other  position  drivers  use  the  independent  jogging  mode  to  command  position.  This 
requires  an  introduction  of  an  additional  loop  to  the  existing  control  system  as  shown  in 
Figure  9-3. 


Figure  9-3.  External  closed-loop  control  diagram 

The  value  of  K  is  determined  experimentally  and  its  values  are  tuned  to  provide 
stable,  no-steady-state-error  operation  of  each  of  the  joints  within  a  particular  range  of 
motion  parameters.  The  Puma  762  system  is  capable  of  high-speed  operation  with  great 
accuracy;  however  such  implementation  would  require  a  different  set  of  K  values  and 
potentially  a  more  complicated  control  loop.  The  values  of  K  and  motion  parameters  used 
in  this  project  are  summarized  in  Table  9-2. 
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Table  9-2.  Values  of  the  K  constant  and  corresponding  range  of  motion  parameters 


Joint  # 

K 

Max  Speed 
(end/s) 

Max  Acceleration 
(enc/s2) 

Max  Deceleration 
(enc/s  ) 

1 

34 

1000-10000 

28000-256000 

28000-256000 

2 

34 

1000-10000 

28000-256000 

28000-256000 

3 

34 

1000-10000 

28000-256000 

28000-256000 

4 

34 

250-1000 

28000-256000 

28000-256000 

5 

15 

250-1000 

28000-256000 

28000-256000 

6 

14 

250-1000 

28000-256000 

28000-256000 

To  further  illustrate  the  performance  of  the  manipulator  system  within  these  ranges, 
the  MJPS  is  commanded  the  same  position,  [35000,  -35000,  50000,  5500,  6500,  -4000], 
with  three  different  sets  of  motion  parameters.  Keep  in  mind  that  when  this  or  any  other 
driver  component  enters  the  Ready  state,  all  other  components  except  the  Primitive 
Manipulator  and  the  sensors  are  set  to  Standby. 

9.3.1  The  “Average”  Set 

Table  9-3  shows  the  set  of  “average”  motion  parameters  used  to  test  the 
performance  of  the  MJPD. 


Table  9-3.  Values  of  the  K  constant  and  “average”  set  of  motion  parameters 


Joint  # 

K 

Max  Speed 
(end/s) 

Max  Acceleration 
(enc/s  ) 

Max  Deceleration 

2 

(enc/s  ) 

1 

34 

5000 

128000 

128000 

2 

34 

5000 

128000 

128000 

3 

34 

5000 

128000 

128000 

4 

34 

1000 

128000 

128000 

5 

15 

1000 

128000 

128000 

6 

14 

1000 

128000 

128000 

The  step  responses  of  each  of  the  joints  as  they  are  commanded  motion  are  shown 


in  Figure  9-4. 
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Figure  9-4.  Joint  step  responses  using  “average”  motion  parameters 
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Figure  9-5.  The  MJPD  screen  as  motion  is  completed  under  the  “average”  set 
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Note  that  only  Joint  5  has  a  slight  overshoot,  and  that  all  the  joints  arrive  within  1 


encoder  count  of  the  desired  position  as  shown  in  Figure  9-5.  This  error  falls  within 


performance  specifications  of  the  Galil  controller. 


9.3.2  The  “Low”  Set 


Table  9-4  shows  the  set  of  “low”  motion  parameters  used  to  test  the  perfonnance  of 
the  Manipulator  Joint  Positions  Driver.  The  step  responses  are  very  similar  to  those  in 
Figure  9-4  with  Joint  5  having  less  overshoot.  However  low  acceleration  and  deceleration 
values  yield  low  response  time,  thus  the  resulting  position  falls  within  a  larger  margin  of 
error.  As  Figure  9-6  shows,  that  magnitude  of  this  error  is  +/-6  encoder  counts.  The  step 
responses  of  each  of  the  joints  as  they  are  commanded  motion  are  shown  in  Figure  9-7. 


Table  9-4.  Values  of  the  K  constant  and 

“low”  set  of  motion  parameters 

Joint  # 

K 

Max  Speed 
(end/s) 

Max  Acceleration 
(enc/s  ) 

Max  Deceleration 
(enc/s  ) 

1 

34 

1000 

28000 

28000 

2 

34 

1000 

28000 

28000 

3 

34 

1000 

28000 

28000 

4 

34 

250 

28000 

28000 

5 

15 
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28000 

28000 

6 
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28000 
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Figure  9-6.  The  MJPD  screen  as  motion  is  completed  under  the  “low”  set 
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Figure  9-7.  Joint  step  responses  using  “low”  motion  parameters 

9.3.3  The  “High”  Set 

Table  9-5  shows  the  set  of  “high”  motion  parameters  used  to  test  the  performance 


of  the  MJPD.  The  step  responses  of  each  of  the  joints  as  they  are  commanded  motion  are 
shown  in  Figure  9-8.  Fligh  values  of  maximum  speed,  acceleration  and  deceleration 
govern  the  quick  response  to  the  commanded  position. 
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Table  9-5.  Values  of  the  K  constant  and  “high”  set  of  motion  parameters 


Joint  # 

K 

Max  Speed 
(end/s) 

Max  Acceleration 
(enc/s2) 

Max  Deceleration 

2 

(enc/s  ) 

1 

34 

10000 

256000 

256000 

2 

34 

10000 

256000 

256000 

3 

34 

10000 

256000 

256000 

4 

34 

2500 

256000 

256000 

5 

15 
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256000 

6 

14 
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Figure  9-8.  Joint  step  responses  using  “high”  motion  parameters 
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Unlike  the  other  two  cases,  this  “high”  case  starts  to  show  some  signs  of 
instability  in  the  initial  part  of  the  motion,  but  quickly  recovers  with  resulting  position 
error  within  the  Galil  controller  specifications  as  shown  in  Figure  9-9. 
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Figure  9-9.  The  MJPD  screen  as  motion  is  completed  under  the  “high”  set 

Figure  9-10  shows  the  final  configuration  of  the  PUMA  762  manipulator  as  seen  by 


the  Subsystem  Commander  component. 


Figure  9-10.  Subsystem  Commander  screen  showing  the  final  configuration  of  Puma  762 
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9.4  Case  3:  Set  End-Effector  Pose 

This  section  shows  the  performance  of  the  Manipulator  End-Effector  Pose  Driver 
Component.  As  described  in  Chapter  8,  this  component  is  responsible  for  computation  of 
independent  joint  efforts  based  on  the  commanded  position  and  orientation  of  the 
end-effector. 

In  order  to  adequately  perform  the  forward  or  reverse  position  analysis,  the  joint 
angles  that  correspond  to  the  manufacturer’s  specifications  need  to  be  converted  to  the 
kinematic  values  or  vice  versa  using  the  following  relationship: 

f)  —  K  0  +  K  (9-2) 

^ i , kinematic  v  1  ^i, manufacturer  v2 

where  values  of  Kl  correspond  to  the  conversion  factors  shown  in  column  3  of 
Table  9-1,  and  values  ofA2are  [0,  90,  90,  180,  180,  0]  for  Joints  1,  2,  3,  4,  5,  and  6 
respectively.  The  orientation  of  the  end-effector  is  expressed  using  a  unit  quaternion. 
Simple  approach  outlined  in  Section  2.3.5  is  used  to  convert  the  four  values  defining  a 
quaternion  into  the  F  a  61  and  F  S6  vectors.  Once  the  analysis  is  complete,  the  function 

returns  up  to  eight  possible  solution  sets.  An  optimization  technique  is  used  to  pick  one 
solution  by  defining  a  total  cost  function  given  in  Equation  9-3: 


mino  s  Cost  = 


( Pi.des  P  i'Ciirr') 


Thus,  the  solution  requiring  the  least  amount  of  encoder  counts  to  get  to  the  desired 
position  and  orientation  is  chosen.  Those  angles  are  then  converted  back  to  the 
manufacturer’s  values  required  in  the  calculation  of  the  desired  joint  efforts.  Hence,  the 
results  of  the  commanded  position  [908,  -602,  512]  mm  and  orientation  [0.4650,-0.1810, 
-0.8030,-0.3320]  are  shown  in  Figure  9-11. 
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Manipulator  End-Effector  Pose  Driuer  Screen 
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Figure  9-11.  The  MEEPD  screen  as  it  responds  to  Set  End-Effector  Pose  message 


The  computation  of  errors  associated  with  the  resulting  position  and  orientation  is 


non-trivial  and  is  not  covered  in  this  thesis.  However,  it  should  be  noted  that  the  resulting 


joint  angle  positions  are  within  2  encoder  counts  with  respect  to  those  computed  using  the 


reverse  position  analysis. 


9.5  Case  4:  Set  Joint  Velocities 

This  scenario  is  very  similar  to  that  covered  in  Section  9.2  since  joint  efforts  are 
interpreted  as  joint  velocities.  The  only  difference  is  that  joint  velocities  are  now 
converted  back  to  joint  efforts  and  then  back  to  velocities  at  the  interface  level.  Figure 
9-12  shows  the  Manipulator  Joint  Velocities  Driver  screen  during  the  commanded 
motion.  The  individual  joint  velocities  are  calculated  using  Equation  6-1  with  “average” 
parameter  set  defining  maximum  joint  velocity.  Therefore,  a  Set  Joint  Velocities  message 
commanding  [-3000,  2500,  -3500,  500,  -300,  250]  for  joints  1,  2,  3,  4,  5,  and  6 
respectively  yields  [-60,  50,  -70,  50,  -30,  25]  joint  efforts  as  shown  in  Figure  9-12. 
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Manipulator  Joint  Uelocities  Driuer  Screen  Commanded  Uelocity 


MJUD  State: 

Ready 

Joint  1 : 

-3000 

MJUD  Instance  ID: 

1 

Joint  2: 

2500 

MJUD  Component  ID: 

56 

Joint  3: 

-3500 

MJUD  Node  ID: 

1 

Joint  4: 

500 

MJUD  Subsystem  ID: 

200 

Joint  5: 

-300 

MJUD  Update  Rate: 

333.01 

Joint  6: 

250 

Queried  Information: 

effort 

uelocity 

Resulting 

Effort 

Joint  1 : 

-59.66 

-2983 

Joint  1 : 

-60.00 

Joint  2: 

49.94 

2497 

Joint  2: 

50.00 

Joint  3: 

-69.94 

-3497 

Joint  3: 

-70.00 

Joint  4: 

50.30 

5  03 

Joint  4: 

50.00 

Joint  5: 

-29.60 

-296 

Joint  5: 

-30.00 

Joint  6: 

25.20 

252 

Joint  6: 

25.00 

Figure  9-12.  The  MJVD  Screen  as  it  responds  to  Set  Joint  Velocities  message 

It  is  apparent  that  very  close  values  of  velocity  are  obtained  with  the  largest  error  of 


0.5%  occurring  in  motion  of  joint  1.  This  error  is  inherent  to  the  theory  behind  the 


independent  jogging  motion,  since  the  “JG”  command  is  directly  passed  to  the  Galil 


controller. 


9.6  Case  5:  Set  End-Effector  Velocity  State 

This  section  is  supposed  to  evaluate  the  performance  of  the  Manipulator 
End-Effector  Velocity  State  Driver.  This  component  takes  as  input  the  desired  velocity 
state  of  the  end-effector  and  computes  the  resulting  joint  velocities.  Any  commanded 
motion  will  reach  a  singularity  configuration  if  not  terminated  in  time;  hence  the  motion 
has  to  be  constantly  monitored  and  carefully  selected.  In  addition,  since  the  resulting 
velocities  vary  as  the  end-effector  moves  in  a  commanded  direction,  the  “high”  set  of 
maximum  parameters  is  used  in  for  this  application  allowing  for  a  wide  range  of  possible 
solutions.  Once  determined,  the  joint  velocities  are  converted  into  efforts  and  passed  to 
the  PM.  The  collection  of  results  for  this  section  is  still  in  progress  and  might  not  be 


included  in  this  document  due  to  the  time  constraints. 
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9.7  Case  6:  Set  Joint  Motion 

This  case  is  in  many  ways  similar  to  Case  2,  in  that  it  uses  the  same  approach  to 
compute  the  resulting  joint  efforts.  However,  the  major  difference  is  seen  as  this 
component  sets  the  parameters  defining  the  trapezoidal  trajectory  profile  with  each  pose. 
As  a  result,  neither  of  the  “average”,  “low”  or  “high”  sets  are  used  in  this 
implementation,  but  rather  a  custom  set  is  created  with  each  pose.  In  this  example,  the 
SSC  sends  a  message  to  the  Manipulator  Joint  Move  Driver  containing  three  different 
poses.  The  results  as  well  as  the  resulting  trajectory  profiles  are  covered  in  the  following 
sections. 

9.7.1  Set  Joint  Motion:  Pose  1 

The  snapshot  of  the  MJMD  screen  after  the  completion  of  the  first  pose  is  shown  in 
Figure  9-13. 


Manipulator  Joint  Moue  Driuer  Screen 


Commanded  Motion 


MJMD  State: 

Ready 

Joint  1 : 

45000 

MJMD  Instance  ID: 

1 

Joint  2: 

45000 

MJMD  Component  ID: 

58 

Joint  3: 

45000 

MJMD  Node  ID: 

1 

Joint  4: 

5000 

MJMD  Subsystem  ID: 

200 

Joint  5: 

5000 

MJMD  Update  Rate: 

334.34 

Joint  6: 

5000 

Commanded  Joint  Ualues:  max  uel  max  acc  max  dec 


Current  pose: 

1 

Joint  1 

4886 

135718 

135718 

Joint  2 

4886 

135718 

135718 

Number  of  poses: 

3 

Joint  3 

4886 

135718 

135718 

Joint  4 

977 

135718 

135718 

Next  pose  time: 

30 

Joint  5 
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135718 

135718 

Joint  6 
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135718 

135718 

Relative  time: 

28.06 

Queried  Information 
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position 

uelocity 

Resulting 

Effort 

Joint  1 

0.00 

44999 

0 

Joint  1 : 

0.03 

Joint  2 

0.00 

44999 

0 

Joint  2: 

0.03 

Joint  3 

0.00 

45001 

0 

Joint  3: 

-0.03 

Joint  4 

0.00 

5001 

0 

Joint  4: 
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0.09 

Figure  9-13.  The  MJMD  screen  showing  component  status  upon  completion  of  pose  1 
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The  desired  trapezoidal  motion  parameters  are  listed  above.  Figure  9-14  shows  the 
velocity  performance  plot  displaying  the  actual  motion  parameters  for  each  of  the  joints. 


- \ 

\ 

\ 

\ 

V 

0  5  10  15  20  25 

Figure  9-14.  Joint  performance  plots  for  motion  parameters  set  in  pose  1 

It  is  apparent  that  these  plots  closely  follow  the  desired  parameters  with  the 


exception  to  the  deceleration  stage  which  is  affected  by  the  additional  control  loop  that 
was  introduced  in  Section  9-3.  In  this  stage,  the  component  constantly  reduces 
commanded  velocity  values  that  are  then  passed  to  the  Galil  controller  which  then  once 
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again  uses  a  trajectory  generator  to  close  the  loop.  The  incremental  position  parameter 
used  in  the  Galil  “JG”  function  is  not  specified,  thus  the  controller  uses  very  small  values 
to  set  the  path  by  default.  The  combination  of  the  two  control  loops  provides  additional 
damping.  Similar  behavior  can  be  observed  in  the  upcoming  sections.  Referring  back  to 
Figure  9-13,  it  can  be  seen  that  the  resulting  positions  are  within  +/- 1  encoder  counts. 
9.7.2  Set  Joint  Motion:  Pose  2 

The  snapshot  of  the  MJMD  screen  after  the  completion  of  the  second  pose  is  shown 
in  Figure  9-15. 


Manipulator  Joint  Moue  Driver*  Screen  Commanded  Motion 


MJMD  State: 

Ready 

Joint  1 : 

0 

MJMD  Instance  ID: 

1 

Joint  2: 

0 

MJMD  Component  ID: 

58 

Joint  3: 

0 

MJMD  Node  ID: 

1 

Joint  4: 

0 

MJMD  Subsystem  ID: 
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Joint  5: 

0 
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Joint  6: 

0 
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2 

Joint  1: 

3257 

21715 

21715 

Joint  2: 
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21715 

Number  of  poses: 

3 

Joint  3: 

3257 

21715 

21715 

Joint  4: 

543 

21715 

21715 

Next  pose  time: 

60 

Joint  5: 

543 

21715 

21715 

Joint  6: 

543 

21715 

21715 

Relative  time: 

53.83 

Queried  Information: 

effort 

position 

velocity 

Resulting 

Effort 

Joint  1 : 

0.00 

1 

0 

Joint  1 : 

-0.03 

Joint  2: 

0.00 

1 

0 

Joint  2: 

-0.03 

Joint  3: 

0.00 

1 

0 

Joint  3: 

-0.03 

Joint  4: 

0.00 

2 

0 

Joint  4: 

-0.19 

Joint  5: 

0.00 

-2 

0 
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0.19 
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0.00 

3 
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Figure  9-15.  The  MJMD  screen  showing  component  status  upon  completion  of  pose  2 
The  second  set  of  joint  positions  goes  in  effect  after  30  seconds  of  the  component 


startup.  The  Puma  was  commanded  to  go  back  to  its  home  position,  which  was 


successfully  accomplished  in  a  little  over  23  seconds.  The  desired  trapezoidal  motion 


parameters  for  pose  2  are  listed  in  Figure  9-15.  Figure  9-16  depicts  the  velocity 


performance  plot  showing  the  actual  motion  parameters  for  each  of  the  joints. 
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Figure  9-16.  Joint  performance  plots  for  motion  parameters  set  in  pose  2 

The  commanded  parameters  resulting  in  the  above  plots  are  very  similar  to  those 
defining  the  “low”  set,  thus  implying  a  higher  position  error  of  +/- 3  encoder  counts. 

9.7.3  Set  Joint  Motion:  Pose  3 


The  snapshot  of  the  MJMD  screen  after  the  completion  of  pose  3  is  shown  in 
Figure  9-17.  The  set  of  joint  positions  goes  in  effect  after  60  seconds  of  the  component 
startup.  The  desired  trapezoidal  parameters  provide  a  mix  of  values  (Figure  9-16). 
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Manipulator  Joint  Moue 

Driver  Screen 

Commanded  Motion 

MJMD  State: 

Ready 

Joint 

1 

-62000 

MJMD  Instance  ID: 

1 

Joint 

2 

-65000 

MJMD  Component  ID: 

58 

Joint 

3 

65000 

MJMD  Node  ID: 

1 

Joint 

4 

5000 

MJMD  Subsystem  ID: 

200 

Joint 

5 

5000 

MJMD  Update  Rate: 

19.33 

Joint 

6 

-5000 

Commanded  Joint  Ualues: 

max_uel 

maxacc 

max_dec 

Current  pose: 

3 

Joint  1 : 

1086 

217148 

92288 

Joint  2: 

1086 

217148 

92288 

Number  of  poses: 

3 

Joint  3: 

1086 

217148 

92288 

Joint  4: 

326 

217148 

92288 

Next  pose  time: 

60 

Joint  5: 

326 

217148 

92288 

Joint  6: 

326 

146518 

92288 

Relative  time: 

140.66 

Queried  Information: 

effort 

position 

velocity 

Resulting 

Effort 

Joint  1: 

0.00 

-61996 

0 

Joint  1 : 

-0.12 

Joint  2: 

0.00 

-64997 

0 

Joint  2: 

-0.09 

Joint  3: 

0.00 

64996 

0 

Joint  3: 

0.12 

Joint  4: 

0.00 

4996 

0 

Joint  4: 

0.39 

Joint  5: 

0.00 

5004 

0 

Joint  5: 

-0.37 

Joint  6: 

0.00 

-4995 

0 

Joint  6: 

-0.45 

Figure  9-17.  The  MJMD  screen  showing  component  status  upon  completion  of  pose  3 
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Figure  9- 18a.  Joint  performance  plots  for  motion  parameters  set  in  pose  3 
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Figure  9- 18b.  Joint  performance  plots  for  motion  parameters  set  in  pose  3 

Commanded  joint  positions  [-62000,  -65000,  65000,  5000,  5000,  -5000]  are 
reached  in  approximately  80  seconds  with  the  accuracy  of  +/-5  encoder  counts.  Figures 
9- 18a  and  9- 18b  show  the  velocity  performance  plot  showing  the  actual  motion 
parameters  for  each  of  the  joints. 

9.8  Case  7:  Set  End-Effector  Path  Motion 
This  section  tests  the  capabilities  of  the  Manipulator  End-Effector  Discrete  Pose 
Driver.  This  component  can  be  considered  a  superset  of  the  MEEPD  in  that  it 
fundamentally  performs  the  same  function  with  the  ability  of  the  user  to  specify  the 
end-effector  path  or  profile.  In  other  words,  this  component  takes  as  input  the  message 
that  specifies  multiple  positions  and  orientations  of  the  end-effector  at  different  times.  A 
test  sample  message  contains  five  different  end-effector  pose  sets,  the  two  of  which  are 
shown  in  Figure  9-19  and  Figure  9-20.  The  “average”  set  of  motion  parameters  is  used 
for  this  component  by  default  and  thus  the  accuracy  of  +/-  3  encoder  counts  was  obtained 
with  all  the  results.  This  chapter  has  successfully  shown  that  all  the  information 
communicated  between  the  JAUS  components  is  done  correctly.  It  has  also  illustrated 
that  each  component  properly  performs  its  required  tasks,  outlined  in  Chapters  4-7. 
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Manipulator  End-Effector  Discrete  Pose  Driuer  Screen  Commanded  Pose: 
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Figure  9-19.  The  MEEDPD  screen  after  completion  of  pose  1 
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Figure  9-20.  The  MEEDPD  screen  after  completion  of  pose  5 
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Finally,  the  chapter  defined  a  range  of  joint  motion  parameters  which  will  ensure 
stable,  minimal  steady-state-error  operation  of  the  Pum  762  manipulator  system.  The  last 
chapter  summarizes  all  the  information  covered  in  this  thesis  and  offers  some  ideas 
regarding  future  component  development  and  alternative  platform  implementations. 


CHAPTER  10 

CONCLUSIONS  AND  FUTURE  WORK 

10.1  Conclusions 

Our  study  focused  on  the  design  and  development  of  9  components  used  to 
command  and  control  a  manipulator  ann.  Considering  that  these  components  are 
developed  for  the  Joint  Architecture  for  Unmanned  Systems  (JAUS),  this  development 
had  to  meet  three  specific  objectives.  First,  each  of  the  components  had  to  meet  the 
criteria  of  a  fully  defined  JAUS  component.  Second,  a  testing  platfonn  had  to  be  chosen, 
and  the  functionality  of  the  manipulator  components  had  to  be  applied  to  the  test 
platform.  Finally,  the  performance  of  this  platfonn  had  to  be  tested  under  the 
implementation  of  the  new  architecture. 

A  standard  JAUS  component  is  defined  through  a  set  of  4  criteria:  the  function  of 
the  component,  the  accepted  input  and  output  messages  that  the  component  may  handle, 
and  the  full  description  of  the  component.  In  these  terms,  all  9  components  were  fully 
constrained  to  the  JAUS  architecture.  The  function  was  clearly  defined  for  each  of  the 
components,  varying  from  the  simple  open-loop  control,  to  the  discrete  closed-loop 
end-effector  position  control.  A  full  set  of  input  and  output  messages  were  defined  to 
meet  the  JAUS  message  standards.  The  component  description  defined  the 
implementation  of  these  components  into  the  JAUS  architecture,  set  the  corresponding 
and  unique  identification  numbers,  and  discuessed  the  specifics  of  their  individual 
functionality. 
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The  Puma  762  robot  was  chosen  as  the  testing  platfonn  due  to  the  availability  and 
acceptable  operating  condition.  The  robot  was  reverse  engineered  prior  to  this 
implementation  to  accommodate  a  commercially  available  Galil  DMC-2100  motion 
controller.  Some  of  the  safety  functionality  of  the  native  Val  II  controller  was  kept  to 
ensure  that  proper  initialization  routines  are  accomplished. 

The  implementation  of  the  Primitive  Manipulator  component  was  used  to  interpret 
joint  efforts  as  joint  velocities  with  respect  to  the  commands  that  the  Galil  controller  used 
to  perpetuate  motion.  This  approach  uses  an  independent  jogging  mode  inherent  to  the 
Galil  controller  interface. 

The  scope  of  the  software  development  during  our  study  involved  the  programming 
of  the  JAUS  functional  elements  on  4  levels.  The  design  aspect  took  advantage  of  the 
multithreading  tool  and  applied  it  to  each  of  the  components  allowing  a  single  program 
on  a  node  level  to  spawn  and  run  all  of  the  manipulator  components.  On  the  lowest  level 
a  program  had  to  be  created  to  be  able  to  interface  the  C  based  JAUS  software  to  the 
Galil  machine-based  command  language.  On  the  highest  level,  the  main  program  was 
used  to  handle  the  startup  and  shutdown  routines  of  the  Manipulator  Control  node.  This 
program  was  further  responsible  for  displaying  all  of  the  information  pertaining  to  the 
particular  component  using  the  curses  windows  development  tool  standard  to  many 
Linux  distributions. 

Finally,  the  performance  of  each  of  the  components  was  tested  in  terms  of  the 
guidelines  set  by  the  JAUS  for  component  functionality.  This  was  accomplished  by 
passing  the  specific  commanding  messages  to  each  of  the  components.  The  results  were 
used  to  determine  the  performance  specifications  of  the  Puma  robot  under  the  new 
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modular  architecture.  More  specifically,  the  accuracy  of  the  resulting  position  and 
velocity  is  within  6  enc  and  15  enc/s,  respectively.  Such  errors,  even  though  small,  are 
still  significant  especially  if  high  precision  operation  is  required  by  the  particular 
application.  There  are  3  reasons  that  could  explain  the  inaccuracies.  First,  there  is  a  small 
margin  of  error  inherent  to  the  independent  jogging  mode  as  it  uses  the  trajectory 
generator  for  closed-loop  position  control.  Second,  messages  sent  through  the  node 
manager  lose  some  precision  during  the  process  of  packing  and  unpacking.  Finally,  a 
small  frequency  associated  with  the  interface  thread  responsible  for  communications  with 
the  Galil  controller  provided  delayed  information  to  the  commanding  components 
running  at  much  higher  update  rates. 

Overall,  the  Manipulator  Control  node  containing  9  JAUS  manipulator  components 
was  tested  successfully  on  the  Puma  762  Robot.  It  can  be  assumed  that  these  components 
could  be  now  implemented  on  any  serial  manipulator  system  with  slight  modifications. 
Moreover,  infrastructure  in  place  allows  for  both  teleop  and  autonomous  control  of  an 
independent  ann,  as  well  as  an  arm  that  would  be  a  part  of  the  vehicle-manipulator 
system. 

10.2  Future  Work 

This  implementation  has  successfully  addressed  the  aspects  of  low-level  and 
mid-level  controls  of  manipulators.  More  specific  messages  oriented  towards  advanced 
task  completion  could  be  added  to  the  current  node.  The  next  major  step  would  be  to  test 
this  implementation  on  a  vehicle-manipulator  system  requiring  both  teleop  and 
autonomous  control,  as  this  implementation  was  only  able  to  address  one  of  the  two 


aspects. 


APPENDIX  A 

EQUATIONS  FOR  A  SPHERICAL  HEPTAGON 


Table  A-l.  Fundamental  fonnulas  for  a  Spherical  Heptagon 


y 

^  12345 

=  ^67^6 

Y 

1 12345 

=  ‘y67C6 

7 

^  12345 

=  C67 

y 

^  23456 

=  SnS7 

Y 

1  23456 

=  si\ci 

7 

^23456 

=  c71 

Y 

^  34567 

=  ^12*^1 

Y 

1  34567 

=  S12C\ 

7 

^34567 

=  C12 

Y 

^  45671 

=  S23S2 

Y 

1  45671 

=  s23c2 

7 

^  45671 

=  C23 

Y 

^  56712 

=  S34S3 

Y 

1  56712 

=  ^34^3 

7 

^  56712 

=  C34 

Y 

^  67123 

=  S45S4 

Y 

1  67123 

=  ^45C4 

7 

^  67123 

=  C45 

Y 

^  71234 

=  *56*5 

7 

71234 

=  *56C5 

7 

^71234 

=  C56 

Y 

^  54321 

r- 

>3 

'O 

ii 

7 

1  54321 

O 

r- 

ii 

7 

^54321 

=  C67 

Y 

^  65432 

=  snsi 

7 

J 65432 

=  slxcx 

7 

^  65432 

=  C71 

Y 

^  76543 

=  $12$  2 

7 

J 76543 

=  snc2 

7 

^  76543 

=  C12 

Y 

^  17654 

=  ^23^3 

7 

J  17654 

=  ^23C3 

7 

^17654 

=  C23 

Y 

^  21765 

=  S34U 

7 

1  21765 

=  S34C4 

7 

^21765 

=  C34 

Y 

^  32176 

=  S45S5 

7 

^  32176 

=  ■NsG 

7 

^ 32176 

=  ^45 

Y 

^  43217 

=  S56S6 

7 

1  43217 

=  S56C6 

^  43217 

=  C56 

102 


103 


Table  A-2.  Subsidiary  fonnulas  for  a  Spherical  Heptagon  Set  1 


^12345 

=  ^6 

—  X  12345 

II 

0^1 

7 

^  12345 

=  ^6 

^23456 

=  A? 

—  X  23456 

=  Yn 

7 

^  23456 

y 

^  34567 

=  *i 

—  X  34567 

=  Yi 

7 

^34567 

=  ^2 

^45671 

=  ^2 

—  X  45671 

=  y2 

7 

^  45671 

=  ^3 

y 

^  56712 

=  ^3 

—  X  56712 

=  y3 

7 

^  56712 

=  ^4 

Y 

^  67123 

=  ^4 

—  X  67123 

=  y4 

7 

^  67123 

=  ^5 

^71234 

=  ^5 

—  X  71234 

=  y5 

7 

^  71234 

=  ^6 

Y 

^  54321 

—  X  54321 

=  Y7 

7 

^  54321 

=  z7 

Y 

^  65432 

=*1 

—  X  65432 

=  Yt 

7 

^  65432 

=  Zi 

Y 

^  76543 

—  X  76543 

=  Y2 

7 

^  76543 

=  z2 

Y 

^  17654 

=  ^3 

~  X  17654 

=  y3 

7 

^ 17654 

=  z3 

Y 

^  21765 

—  X  21765 

=  y4 

7 

^21765 

=  ^4 

Y 

^  32176 

—  X  32176 

=  y5 

7 

^  32176 

=  ^5 

Y 

^  43217 

=  ^6 

—  X  43217 

=  y6 

^  43217 

=  Zt 

Other  sets  related  to  this  configuration  as  well  as  the  derivations  of  the  direction 
cosines  for  the  spatial  heptagon  are  listed  in  the  Appendix  section  of  [9]. 


APPENDIX  B 

JAUS  MANIPULATOR  COMPONENTS:  SOURCE  CODE 


B.l  The  Galillnterface.c  File  and  the  Corresponding  Header  Galillnterface.h 


///////////////////////////////////////////////////////////////////////////////////////// 


//  File: 

//  Version: 

//  Written  by: 
//  Template  by: 
//  Date: 

// 

//  Description: 
// 


galillnterface . c 

0 . 1  Original  Creation 

Ognjen  Sosa  (ognjensosa@hotmail.com) 

Tom  Galluzzo  (galluzzt@ufl.edu) 

09/28/2004 

This  file  contains  the  C  code  used  to  interface  JAUS  to  PUMA  762 
manipulator  and  the  GALIL  DMC2100  controller. 


///////////////////////////////////////////////////////////////////////////////////////// 


#include 

Cctype . h> 

#include 

<stdlib . h> 

#include 

<stdio . h> 

#include 

<string . h> 

#include 

<math . h> 

#include 

<errno . h> 

#include 

<sys/time . h> 

#include 

<unistd. h> 

#include 

<pthread. h> 

#include 

"galillnterface . h" 

#include 

"mcConstants . h" 

// 

#include 

"logLib . h" 

// 

#include 

"timeLib . h" 

// 

#include 

"dmclnx . h" 

// 

#include 

"RoboTalk . h" 

// 

#include 

"me . h" 

// 

Constants  used  across  Manipulator  Control  Node 
Debug  functions 
Timing  functions 

Interface  to  the  Galil  Controller 
Interface  to  the  RoboWorks  software 
Defines  getRoboWorksReady ( )  function 


//  Controller  Variables 
long  rc  =  0; 

char  buffer  [32]  =  ""; 

char  input_string [ 128 ]  = 

HANDLE DMC  hdmc  =  -1; 

int 

CONTROLLERINFO 


//  Return  code 

//  Response  from  the  controller 
;//  Command  to  the  controller 
//  Handle  to  controller 
f InMotion [NUM_JOINTS] ;  //  Motion  complete  flag 
controllerinfo;  //  Controller  information  structure 


char 


RV [ ]  ={0x12, 0x16, ' \r' , 0x0} 


//  RoboTalk  Variables 

char  filename []  =  "Puma760Robot" ;  //  Not  case-sensitive 

char  ipAddress[]  =  "192.168.0.86";  //  ipAddress  of  the  computer  on 

//which  RoboWorks  is  running 

float  tagValues [NUM_JOINTS] ; 

char*  TagNames[]  =  {"Puma_Jl",  "Puma_J2",  "Puma_J3",  "Puma_J4",  "Puma_J5",  "Puma_J6"}; 


//  Ready  Variables 

int  interfaceReady  =  0,  initialReady  =  0,  homeReady  =  0; 
int  galilReady  =  0,  valReady  =  0,  armReady  =  0; 


//  Interface 

pthread_t 

int 

int 

double 


Thread  Variables 

inter faceThreadld; 
inter faceRun; 
inter faceThreadRunning 
interfaceThreadHz  =  0; 


FALSE; 


//  Other  Variable  Declarations 

static  double  jointCmdEf fort [NUM_JOINTS] ,  jointRetEf fort [NUM_JOINTS] ; 
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static  double  retPosition [NUM_JOINTS] ,  retPositionEnc [NUM_JOINTS ] , 
retPositionAbs [NUM_JOINTS] ; 

static  double  retVelocity [NUM_JOINTS] ,  retForceTorque [NUM_JOINTS] , 
read_velocity [NUM_JOINTS]  ; 

static  double  maxSpeed [NUM_JOINTS] ,  maxAcceleration [NUM_JOINTS]  , 
maxDeceleration [NUM_JOINTS ] ; 


int  interfaceStartup (void) 

{ 

pthread_attr_t  attr; 
int  i; 

long  index [NUM_JOINTS] ; 

//  Initialize  variables 
for  ( i=0 ;  i<NUM_JOINTS ;  i++) 


jointCmdEffort [i]  =  0; 
jointRetEffort [i]  =  0; 
if  (i  <  3)  { 

maxSpeed [i]  =  5000; 

values  assigned 

maxAcceleration [i] 
maxDeceleration [i] 

} 

else  { 

maxSpeed [i]  =  1000; 
maxAcceleration  [  i ] 
maxDeceleration [i] 


//  Use  this  value  as  default  unless  other 

150000; 

150000; 


150000; 

150000; 


fInMotion[i]  =  0; 
index [i]  =  0; 


//  Connect  to  RoboWorks  3D  software 
if  (getRoboWorksReady ( )  ==  1) 

Connect (filename,  ipAddress) ; 

//  Perform  intialization  procedures 
inter faceReady  =  0; 
initManipulator (index)  ; 
sleep  ( 1 ) ; 

//  homeManipulator ( ) ; 

//  sleep  (3)  ; 


//Start  interface  thread 
inter faceRun  =  TRUE; 


pthread_attr_init (Sattr) ; 

pthread_attr_setdetachstate (&attr, PTHREAD_CREATE_DETACHED) ; 
pthread__create  ( &interf aceThreadld,  Sattr,  interfaceThread,  NULL)  ; 
pthread_attr_destroy ( Sattr ) ; 

//  Check  if  initialization  and  homing  procedures  complete  in  order  to  proceed 
if  (getlnitialize ( )  &&  getPumaHome ( )  ) 
interf aceReady  =  1; 
return  0; 

} 


int  interf aceShutdown (void) 

{ 

double  timeOutSec; 
interfaceRun  =  FALSE; 

timeOutSec  =  getTimeSeconds ( )  +  INTERFACE_THREAD_TIMEOUT_SEC; 
while ( interf aceThreadRunning) 

{ 

usleep (10000) ; 

if (getTimeSeconds ( )  >=  timeOutSec) 

{ 

pthread_cancel (interfaceThreadld) ; 
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interf aceThreadRunning  =  FALSE; 

cError ( "interface :  interf aceThread  Shutdown  Improperly\n" ) ; 
break; 


} 

//  Shut  down  motors 

rc  =  DMCCommand (hdmc,  "OP  ?;",  buffer,  sizeof (buffer) ) ;  //Query  actuator  status 
if  (rc) 

PrintError (rc) ; 
else 
{ 

long  temp; 

temp  =  atol (buffer) ; 
if  (temp  ==  1) 

{ 

rc  =  DMCCommand (hdmc,  "OPO;",  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 

} 

} 

inter faceReady  =  0; 

//  Close  connection  to  RoboWorks 
//  if  (getRoboWorksReady ( )  ==  1) 

//  Disconnect () ; 

//  Reset  Galil 
resetGalil ( ) ; 

//  Close  connection  to  Galil 
rc  =  DMCClose (hdmc) ; 
if  (rc) 

{ 

PrintError  (rc) ; 
return  rc; 

} 


return 


0; 


void  *interfaceThread (void  *threadData) 

{ 

double  time  =  0,  prevTime  =  0; 
int  i; 


// 


interfaceThreadRunning  =  TRUE; 
time  =  getTimeSeconds ( ) ; 

while (interfaceRun) 


prevTime  =  time; 

time  =  getTimeSeconds ()  ; 

interf aceThreadHz  =  1 . 0/ (time-prevTime) ; 

motion ( jointCmdEffort,  jointRetEffort) ; 

position (retPosition,  retPositionEnc,  retPositionAbs) ; 

velocity ( retVelocity ) ; 

forcetorque (retForceTorque) ; 

for  ( i=0 ;  i<NUM_JOINTS ;  i++) 


tagValues[i]  =  (float) retPositionAbs [i] ; 

} 

usleep (100) ; 


// 

if 


Sending  position  values  to  RoboWorks 
(getRoboWorksReady ( )  ==  1) 

SetTagValues (TagNames,  tagValues, 


6)  ; 


interfaceThreadRunning  =  FALSE; 
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pthread_exit (NULL) ; 

} 

//  Accessors 

double  getlnterfaceUpdateRate (void) {  return  interfaceThreadHz;  } 

int  getlnterf aceThreadRunning ( void) {  return  interf aceThreadRunning;  } 

double  get JointCmdEf fort (int  i)  {  return  j ointCmdEf f ort [ i ] ;  } 

double  get JointRealEf fort (int  i)  {  return  read_velocity [ i ] * 100/maxSpeed [ i ] ;  } 

double  get JointPosition (int  i)  {  return  retPosition  [ i ] ;  } 

double  get JointVelocity (int  i)  {  return  retVelocity [ i ] ;  } 

double  get JointForceTorque (int  i)  {  return  retForceTorque [ i ] ;  } 

int  getGalilReady (void)  {  return  galilReady;  } 

int  getValReady (void)  {  return  valReady;  } 

int  getArmReady ( void)  {  return  armReady;  } 

int  getlnitialize (void)  {  return  initialReady ;  } 

int  getPumaHome  (void)  {  return  homeReady;  } 

int  getlnterfaceReady (void) {  return  interf aceReady;  } 

//////////////////////////////////////////////////////////////////////////////////// 

//  Private  Functions 

//////////////////////////////////////////////////////////////////////////////////// 
//  Function  used  to  access  values  of  SET_JOINT_EFFORT  from  the  PRIMITIVE  MANIPULATOR 
//////////////////////////////////////////////////////////////////////////////////// 
int  setEf fort (double  *effort) 

{ 

int  i; 

for  (i=0;  i<NUM_JOINTS ;  i++) 

{ 

j ointCmdEf fort [i]  =  effort [i] ; 

} 

return  0; 

} 

/////////////////////////////////////////////////////////////////////////////////// 
//  This  particular  implementation  interprets  joint  effort  as  percentage  of  maximum 
//  velocity.  Thus  in  the  interface  startup  routine,  default  maximum  velocity  for 
//  all  six  joints  is  set  at  5000  enc/s.  This  default  value  is  used  with  all  but  two 
//  JAUS  components  which  dynamically  set  joint  velocities. 

// 

//  As  a  result,  the  function  below  is  currently  set  up  to  overwrite  the  default 
//  velocity  value  if  requested  by  any  of  the  two  components  requiring  this 
//  capability. 

////////////////////////////////////////////////////////////////////////////////// 

void  setMaxSpeed (double  *max_speed) 

{ 

int  i; 

for  ( i=0 ;  i<NUM_JOINTS ;  i++) 

{ 

if  ( i==0 ) 

maxSpeed[i]  =  max_speed[i]  *  Jl_RAD_TO_ENC; 
else  if  (i==l) 

maxSpeed[i]  =  max_speed[i]  *  J2_RAD_TO_ENC; 
else  if  (i==2) 

maxSpeed[i]  =  max_speed[i]  *  J3_RAD_TO_ENC; 
else  if  (i==3) 

maxSpeed[i]  =  max_speed[i]  *  J4_RAD_TO_ENC; 
else  if  (i==4) 

maxSpeed[i]  =  max_speed[i]  *  J5_RAD_TO_ENC; 

else 

maxSpeed[i]  =  max_speed[i]  *  J6_RAD_TO_ENC; 


void  setMaxAcceleration (double  *max_acceleration) 

{ 

int  i; 

for  ( i=0 ;  i<NUM_JOINTS ;  i++) 

{ 


if  ( i==0 ) 
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} 


maxAcceleration 

i 

else 

if 

(i==l) 

maxAcceleration 

i 

else 

if 

(i==2) 

maxAcceleration 

i 

else 

if 

(i==3) 

maxAcceleration 

i 

else 

if 

(i==4 ) 

maxAcceleration 

i 

else 

maxAcceleration 

i 

max_acceleration [ i ] 
max_acceleration [ i ] 
max_acceleration [ i ] 
max_acceleration [ i ] 
max_acceleration [ i ] 
max_acceleration [ i ] 


*  Jl_RAD_TO_ENC; 

*  J2_RAD_TO_ENC; 

*  J3_RAD_TO_ENC ; 

*  J4_RAD_TO_ENC; 

*  J5_RAD_TO_ENC ; 

*  J6  RAD  TO  ENC; 


void  setMaxDeceleration (double  *max_deceleration) 

{ 

int  i; 

for  ( i=0 ;  i<NUM_JOINTS ;  i++) 

{ 


maxDeceleration 

i] 

=  max 

deceleration 

i] 

* 

J1 

_RAD_ 

_T0_ 

_ENC 

else 

if 

(i==l ) 

maxDeceleration 

i] 

=  max 

deceleration 

i] 

* 

J2 

_RAD_ 

_T°_ 

_ENC 

else 

if 

(i==2) 

maxDeceleration 

i] 

=  max 

deceleration 

i] 

* 

J3 

RAD_ 

TO_ 

ENC 

else 

if 

(i==3) 

maxDeceleration 

i] 

=  max 

deceleration 

i] 

* 

J4 

RAD_ 

TO_ 

ENC 

else 

if 

(i==4 ) 

maxDeceleration 

i] 

=  max 

deceleration 

i] 

* 

J5_ 

_RAD_ 

_T0_ 

_ENC 

else 

maxDeceleration 

i] 

=  max 

deceleration 

i] 

* 

J6 

RAD 

TO 

ENC 

//////////////////////////////////////////////////////////////////////////////////// 
//  Function  used  to  convert  incoming  joint  efforts  into  velocity  values  and  send 
//  them  to  the  Galil  controller.  It  returns  actual  effort  values. 
//////////////////////////////////////////////////////////////////////////////////// 
void  motion (double  * jointCmdEffort,  double  * jointRetEffort) 

{ 

double  cmd_velocity [NUM_JOINTS ] ,  curr_velocity [NUM_JOINTS ] , 
read_acceleration [NUM_JOINTS ] ; 
int  i,  j  =65 ; 

//Clip  Command 

for  ( i=0 ;  i<NUM_JOINTS ;  i++) 

{ 

if  ( j ointCmdEf f ort [ i ]  >  100) 

j ointCmdEf f ort [ i ]  =  100; 
if  ( j ointCmdEf fort [ i ]  <  -100) 

j ointCmdEf fort [ i ]  =  -100; 

} 


//Convert  commands  into  velocity  values  in  units  of  enc/s  and  command  motion 
for  ( i=0 ;  i<NUM_JOINTS ;  i++) 

{ 

cmd_velocity [i]  =  ( (jointCmdEffort [i] / 1 0 0 )  *  maxSpeed [ i ] ) ; 

if  ( i<3 )  { 

if  (cmd_velocity [i]  >  10000) 

cmd_velocity [i]  =  10000; 
else  if  (cmd_velocity [i]  <  -10000) 
cmd_velocity [i]  =  -10000; 

} 

else  { 

if  (cmd_velocity [i]  >  2500) 

cmd_velocity [i]  =  2500; 
else  if  (cmd_velocity [i]  <  -2500) 
cmd_velocity [i]  =  -2500; 

} 

sprintf (input_string,  "AC%c=?; ", j ) ; 
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rc  =  DMCCommand (hdmc,  input_string,  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 

read_acceleration [i]  =  atof (buffer) ; 

j++; 

} 

sprint f ( input_string,  "JG  %5 . Olf , %5 . Olf , %5 . Olf , %5 . Olf , %5 . Olf , %5 . Olf ; " , 
cmd_velocity [ 0 ] , cmd_velocity [ 1 ] , cmd_velocity [2 ]  , cmd_velocity [ 3 ] , cmd_velocity [ 4 ]  ,  cmd_veloc 
ity [ 5 ] ) ; 

rc  =  DMCCommand (hdmc,  input_string,  buffer,  sizeof  (buffer) ) ; 
if  (rc) 

PrintError  (rc)  ; 

sprintf ( input_string,  "AC  %7  .  Olf,  %7 .Olf ,  %7 .Olf , %7 . Olf, %7 .Olf , %7  .Olf", 
maxAcceleration [ 0 ] , maxAcceleration [ 1 ] , maxAcceleration [ 2 ] , maxAcceleration [ 3 ] , maxAccelerati 
on [ 4 ] , maxAcceleration [ 5 ] ) ; 

rc  =  DMCCommand (hdmc,  input_string,  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError  (rc)  ; 
for  ( i=0 ;  i<NUM_JOINTS ;  i++) 

{ 

cDebug (10, "Accelerations  are  %lf\n", read_acceleration [i] ) ; 

} 

sprintf (input_string,  "DC  %7 . Olf , %7 . Olf , %7 . Olf , %7 . Olf , %7 . Olf , %7 . Olf ; " , 
maxDeceleration [ 0 ]  ,  maxDeceleration [ 1 ] , maxDeceleration [ 2 ] , maxDeceleration [ 3 ] , maxDecelerati 
on [ 4 ]  ,  maxDeceleration [ 5 ] ) ; 

rc  =  DMCCommand (hdmc,  input_string,  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError  (rc)  ; 

rc  =  DMCCommand (hdmc,  "BG;",  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError  (rc) ; 


//////////////////////////////////////////////////////////////////////////////////// 

//  Function  used  to  look  up  position  sensor  data.  retPositionAbs  is  the  value  used 
//  as  RoboWorks  input  and  has  neccessary  to  replicate  exact  joint  positions. 
//////////////////////////////////////////////////////////////////////////////////// 
void  position (double  *retPosition,  double  *retPositionEnc,  double  *retPositionAbs) 

{ 

int  i,  j  =  65 ; 

double  read_position [NUM_JOINTS ] ; 

//Read  Position 

for  ( i=0 ;  i<NUM_JOINTS ;  i++) 

{ 

sprintf ( input_string,  "TP  % c ; " , j ) ; 

rc  =  DMCCommand (hdmc,  input_string,  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 

read_position [i]  =  atof (buffer) ; 
if  (i  ==  0)  { 

retPosition [ i ]  =  read_position [ i ]  /  Jl_RAD_TO_ENC;  //rad 
retPositionAbs [i]  =  ( read_position [ i ] ) *  JOINTl_ENC_TO_DEG  -  90;  //deg 

} 

else  if  (i  ==  1)  { 

retPosition [ i ]  =  read_position [ i ]  /  J2_RAD_TO_ENC;  //rad 
retPositionAbs [ i ]  =  ( read_position [ i ] ) *  JOINT2_ENC_TO_DEG  -  90; 

/  /deg 

} 

else  if  (i  ==  2)  { 

retPosition [i]  =  read_position [ i ]  /  J3_RAD_TO_ENC;  //rad 
retPositionAbs [ i ]  =  - ( read_position [ i ] ) *  JOINT3_ENC_TO_DEG  -  90  ; 

/  /deg 

} 

else  if  (i  ==  3)  { 

retPosition  [ i ]  =  read_position [ i ]  /  J4_RAD_TO_ENC;  //rad 
retPositionAbs [i]  =  ( ( read_position [ i ] ) *  JOINT4_ENC_TO_DEG) ;  //deg 

} 

else  if  (i  ==  4)  { 
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retPosition [i]  =  read_position [ i ]  /  J5_RAD_TO_ENC;  //rad 
retPositionAbs [i]  =  ( read_position [ i ] )  *  JOINT5_ENC_TO_DEG  ;  //deg 
retPosition [ i ]  =  retPosition [ i ]  +  0 . 5*retPosition [ i-1 ] ; 

} 

else  { 

retPosition [i]  =  read_position [ i ]  /  J6_RAD_TO_ENC;  //rad 
retPositionAbs [i]  =  ( read_position [ i ] )  *  JOINT6_ENC_TO_DEG;  //deg 

} 

retPositionEnc [i]  =  read_position [ i ] ;  //enc 
j++; 


//////////////////////////////////////////////////////////////////////////////////// 
//  Function  used  to  look  up  velocity  sensor  data 
//////////////////////////////////////////////////////////////////////////////////// 
void  velocity (double  *retVelocity) 

{ 

int  i,  j  =65 ; 

//Read  Velocity 

for  ( i=0 ;  i<NUM_JOINTS ;  i++) 


sprintf ( input_string,  "TV  %c;",  j); 

rc  =  DMCCommand (hdmc,  input_string,  buffer,  sizeof (buffer) ) ; 
if  (rc) 


PrintError (rc) ; 

read_velocity [i]  =  atof (buffer) ; 
if  (i  ==  0) 

retVelocity [i]  =  read_velocity [ i ] 
else  if  (i  ==  1) 

retVelocity [i] 
else  if  (i  ==  2) 

retVelocity [i] 
else  if  (i  ==  3) 

retVelocity [i] 
else  if  (i  ==  4) 

retVelocity [i] 

else 


read_velocity [ i ] 
read_velocity [ i ] 
read_velocity [ i ] 
read_velocity [ i ] 


/  Jl_RAD_TO_ENC; 
/  J2_RAD_TO_ENC; 
/  J3_RAD_TO_ENC ; 
/  J4_RAD_TO_ENC; 
/  J5  RAD  TO  ENC; 


// rad/s 
// rad/s 
// rad/s 
// rad/s 
// rad/s 


} 


retVelocity [i]  =  read_velocity [ i ]  /  J6_RAD_TO_ENC;  //rad/s 


j++; 


} 

//////////////////////////////////////////////////////////////////////////////////// 
//  Function  used  to  look  up  force/torque  sensor  data 
// 

//  NOTE;  PUMA  762  Manipulator  does  not  have  torque  sensors  built  in,  thus  this 
//  function  exists  solely  as  an  example  and  for  potential  future  implementation 
//  on  a  system  with  torque  or  force  sensors.  Data  collected  below  is  just  noise 
//  converted  to  imaginary  torque  values. 

//////////////////////////////////////////////////////////////////////////////////// 

void  forcetorque (double  *retForceTorque) 


int  i,  j  =  65 ; 

double  curr_f orcetorque [NUM_JOINTS ] ,  stall_f orcetorque [NUM_JOINTS ] ; 
double  low_limit,  high_limit,  slope [NUM_JOINTS]; 


//Function  Parameters 
low_limit  =  -9.998;  //  Volts 
high_limit  =  9.998;  //  Volts 


stall_forcetorque [0] 
stall_f orcetorque [ 1 ] 
stall_forcetorque [2 ] 
stall_forcetorque [3] 
stall_forcetorque [4] 
stall_forcetorque [5] 


260;  //ft-lb 
377;  //ft-lb 
294;  //ft-lb 
52;  //ft-lb 
52;  //ft-lb 
24;  //ft-lb 


//Read  Torque 

for  ( i=0 ;  i<NUM_JOINTS ; 


i++) 


Ill 


curr_forcetorque [i]  =  0; 

sprintf ( input_string,  "TT  %c;",  j); 

rc  =  DMCCommand (hdmc,  input_string,  buffer,  sizeof (buffer) ) ; 
if  (rc) 


PrintError (rc) ; 

curr_forcetorque [i]  =  atof (buffer) ; 

slope [i]  =  stall_forcetorque [i]  /  (high_limit  -  low_limit) ;  //ft-lb/V 
retForceTorque [ i ]  =  slope [i]  *  curr_f orcetorque [i] *FTLB_TO_NM;  //Nm 

j++; 


int  resetGalil (void) 

{ 

rc  =  DMCReset (hdmc)  ; 
if  (rc) 

{ 

PrintError (rc) ; 
return  rc; 

} 

return  0; 

} 

/////////////////////////////////////////////////////////////////////////// 

//  Function  opening  the  connection  to  the  Galil  controller. 
/////////////////////////////////////////////////////////////////////////// 
void  startGALILController (void) 

{ 

memset (&controllerinfo,  '\0',  sizeof (controllerinfo) ) ; 

controllerinfo . cbSize  =  sizeof  (controllerinfo) ; 
controllerinfo . usModellD  =  MODEL_2100; 

controllerinfo . f ControllerType  =  ControllerTypeEthernet; 
controllerinfo . ulTimeout  =  10000; 
controllerinfo . ulDelay  =  5; 

//  IP;  192.168.0.84  is  the  current  IP  address  of  the  GALIL  controller 
strcpy (controllerinfo . hardware info . socket inf o . szIPAddress ,  "192.168.0.84") 
controllerinfo . hardwareinf o . socket inf o . f Protocol  =  Ether net ProtocolTCP; 

DMCInitLibrary ( ) ; 

//  Open  the  connection 

rc  =  DMCOpen ( Scontrollerinfo,  &hdmc) ; 

if  (rc) 


PrintError  (rc) ; 
galilReady  =  0; 


else 

galilReady  =  1; 

} 

/////////////////////////////////////////////////////////////////////////// 
//  Check  the  status  of  the  VAL  II  controller  maintaing  system  checks 
//  provided  by  the  manufacturer. 

/////////////////////////////////////////////////////////////////////////// 

void  startVALController (void) 

{ 

int  temp  =  0; 

rc  =  DMCCommand (hdmc,  "MO;",  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 

rc  =  DMCCommand (hdmc,  "OPO;",  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 

rc  =  DMCCommand (hdmc,  "AF  0, 0, 0, 0, 0, 0 ; " ,  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 

rc  =  DMCCommand (hdmc,  " IN_3=@ IN [ 3 ] ; " ,  buffer,  sizeof (buffer) ) ; 
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if  (rc) 

PrintError (rc) ; 

rc  =  DMCCommand (hdmc,  "IN_3=?;",  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 
else 


temp  =  atol (buffer) ; 


rc  =  DMCCommand (hdmc, 
if  (rc) 

PrintError  (rc) ; 

rc  =  DMCCommand (hdmc, 

if  (rc) 

PrintError  (rc) ; 

rc  =  DMCCommand (hdmc, 

if  (rc) 

PrintError  (rc) ; 
if  (temp  ==  0) 

valReady  =  0; 

else 


valReady  =  1; 


"AT  -1000;",  buffer,  sizeof (buffer) ) ; 

"SH;",  buffer,  sizeof (buffer) ) ; 

"OP1;",  buffer,  sizeof (buffer) ) ; 


/////////////////////////////////////////////////////////////////////////// 
//  Check  the  status  of  the  arm  power  (remote  switch  capable) . 
/////////////////////////////////////////////////////////////////////////// 
void  startARM ( void) 


int  temp  =  0; 


rc  =  DMCCommand (hdmc,  " IN_1=@ IN [ 1 ] ; " ,  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 

rc  =  DMCCommand (hdmc,  "IN_1=?;",  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 

else 


temp  =  atol (buffer) ; 

} 

rc  =  DMCCommand (hdmc,  "AT  -1000;", 
if  (rc) 

PrintError  (rc) ; 
if  (temp  ==  0) 

armReady  =  0; 

else 


armReady  =  1; 


buffer,  sizeof (buffer) ) ; 


/////////////////////////////////////////////////////////////////////////// 

//  Joint  initialization  is  done  by  checking  agains  the  stored  voltage  ratio 
//  values  that  correspond  to  a  particular  index.  The  function  further  finds 
//  the  closest  index  thus  allowing  the  user  to  know  the  exact  position  of 
//  the  manipulator  in  any  starting  configuration. 
/////////////////////////////////////////////////////////////////////////// 
void  initManipulator ( long  *index) 

{ 

int  i,  j; 
char  k  =  65; 

double  min=0,  dif f [NUM_JOINTS ] [NUM_INDEX] ,  pot [NUM_JOINTS ] , 
stored_ratio [NUM_JOINTS] [NUM_INDEX] ,  current_ratio [NUM_JOINTS ] ,  power [NUM_JOINTS ] , 
potpower ; 

long  pos [NUM_JOINTS] ,  home_index [NUM_JOINTS ] ,  x=0,  y=0,  z=0,  w=0; 
char  index_string [ 32 ] ; 

for  ( i=0 ;  i<NUM_JOINTS ;  i++) 

{ 

current_ratio [i]  =  0; 
fInMotion[i]  =  1; 
pos[i]  =  0; 
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if  (i<3) 


home 

index | 

[i]  = 

35 

else 

if 

(i== 

:3) 

home 

index | 

[i]  = 

35 

else 

if 

(i== 

:4 ) 

home 

index | 

[4]  = 

16 

else 

home 

index | 

[5]  = 

15 

for 

f 

<j  = 

0;  j  <70 ;  j++) 

1 

stored  ratio [i] 

[j] 

diff 

[i]  [j] 

=  0; 

} 

} 

averagePotPower (pot,  &potpower) ; 

for  ( i=0 ;  i<NUM_JOINTS ;  i++) 

{ 

if (pot [i] >2 . 45) 

{ 

sprint f ( index_string,  " JG%c=-500 ; " , k) ; 

rc  =  DMCCommand (hdmc,  index_string,  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 

} 

else 

{ 

sprint f ( index_string,  " JG%c=500 ; " , k) ; 

rc  =  DMCCommand (hdmc,  index_string,  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 


k++; 

} 

sprintf (input_string,  "FI  ABCDEF" ) ; 

rc  =  DMCCommand (hdmc,  input_string,  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 

sprintf ( input_string,  "BG  ABCDEF;"); 

rc  =  DMCCommand (hdmc,  input_string,  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc)  ; 

while  (  !  rc  &&  fInMotion[0]  &&  fInMotion[l]  &&  fInMotion[2]  &&  fInMotion[3]  && 
fInMotion[4]  &&  f InMotion [5] ) 

{ 

for  ( 1  =  0 ;  i <NUM_ JO I NT  S ;  i++) 

{ 

j  =  65; 

sprintf ( input_string,  "MG_BG%c;",  j); 
rc  =  DMCCommand (hdmc,  input_string,  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 

else 

f InMotion [i]  =  atoi (buffer) ; 

j++; 

} 

} 

rc  =  DMCCommand (hdmc,  "AM  ABCDEF",  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 

rc  =  DMCCommand (hdmc,  "HX1;",  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 

rc  =  DMCCommand (hdmc,  "SP  1000,1000,1000,1000,1000,1000;", 
sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc)  ; 


buffer 
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collectRatios (stored_ratio) ; 
k  =  65; 

for  ( i=0 ;  i<NUM_JOINTS ;  i++) 

{ 

min  =  fabs(pot[i]  -  power [i] ); 
for  ( j  =0 ;  j<NUM_INDEX;  j++) 

{ 

power [i]  =  potpower  *  stored_ratio [ i ] [ j ] ; 
diff[i][j]  =  fabs(pot[i]  -  power[i]); 
if (dif f [ i ] [j]  <  min) 

{ 

index [i]  =  j; 
min  =  diff [i] [ j ] ; 

} 

} 


if  (i 

==  0) 

x  =  3200, 

z  =  3200, 

y  = 

1900,  w  =  -1300 

else 

if  (i  ==  1) 
x  =  3200, 

z  =  3200, 

y  = 

0,  w  =  -3200; 

else 

if  (i  ==  2) 
x  =  3200, 

z  =  3200, 

y  = 

1200,  w  =  -2000 

else 

if  (i  ==  3) 
x  =  1000, 

z  =  1000, 

y  = 

200,  w  =  -800; 

else 

if  (i  ==  4) 
x  =  1000, 

z  =  1000, 

y  = 

0,  w  =  -1000; 

else 

x  =  1000, 

z  =  1000, 

y  = 

300,  w  =  -700; 

if  (index [i]  <=  home_index [ i ] ) 

pos[i]  =  (home_index [ i ]  -  index [i] )  *  x  +  y; 

else 

pos[i]  =  - (index [i]  -  home_index [ i ]  -  1)  *  z  +  w; 

sprintf ( input_string,  "DP%c=%ld",  k,  -pos[i]); 
rc  =  DMCCommand (hdmc,  input_string,  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 

k++; 

} 

rc  =  DMCCommand (hdmc,  "SP  3000,3000,3000,1000,1000,1000;",  buffer, 
sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 
initialReady  =  1; 


/////////////////////////////////////////////////////////////////////////// 

//  Home  the  manipulator. 

/////////////////////////////////////////////////////////////////////////// 

void  homeManipulator (void) 

{ 

int  i,  j  =65 ; 

double  cmd_position [NUM_JOINTS ] ,  curr_position [NUM_JOINTS ]  ; 

for  ( i=0 ;  i<NUM_JOINTS ;  i++) 

{ 

fInMotion[i]  =  1; 

sprintf ( input_string,  "TP  % c ; " , j ) ; 

rc  =  DMCCommand (hdmc,  input_string,  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 

curr_position [i]  =  atof (buffer) ; 
cmd_position [i]  =  0; 
if  (i  <  3) 

{ 

if  (fabs (curr_position [i] )  >  112000)  homeReady  =  0; 

} 

else  if  (i  ==  3) 

{ 

if  (fabs (curr_position [i] )  >  35000)  homeReady  =  0; 
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} 

else  if  (i  ==  4) 

{ 

if  (fabs (curr_position [i] )  >  16000)  homeReady  =  0; 

} 

else 

{ 

if  (fabs (curr_position [i] )  >  15500)  homeReady  =  0; 

} 

j++; 

} 

rc  =  DMCCommand (hdmc,  "SP  3000,3000,3000,1000,1000,1000;",  buffer, 
sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 

sprintf (input_string,  "PA  0,  0,  0,  0,  0,  0;  " )  ; 

rc  =  DMCCommand (hdmc,  input_string,  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 

sprintf ( input_string,  "BG  ABCDEF;"); 

rc  =  DMCCommand (hdmc,  input_string,  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 

while  ( ! rc  &&  fInMotion[0]  &&  fInMotion[l]  &&  fInMotion[2]  &&  fInMotion[3]  && 
fInMotion[4]  &&  f InMotion [ 5 ] ) 

{ 

for  ( i=0 ;  i<NUM_JOINTS ;  i++) 

{ 

j  =  65; 

sprintf ( input_string,  "MG_BG%c;",  j); 
rc  =  DMCCommand (hdmc,  input_string,  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 

else 

f InMotion [i]  =  atoi (buffer) ; 

j++; 


} 

rc  =  DMCCommand (hdmc,  "AM  ABCDEF",  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError  (rc) ; 
homeReady  =  1 ; 

} 

/////////////////////////////////////////////////////////////////////////// 
//  Obtain  voltage  ratio  values  of  the  Galil  EEPROM 
/////////////////////////////////////////////////////////////////////////// 
void  collectRatios (double  stored_ratio [NUM_JOINTS] [NUM_INDEX] ) 

{ 

int  i,  j,  k  =  65; 

for  ( i=0 ;  i<NUM_JOINTS ;  i++) 

{ 

for  ( j  =0 ;  j  <NUM_INDEX;  j++) 

{ 

sprintf (input_string,  "POT%c [ %d] =?" ,  k,  j); 
rc  =  DMCCommand (hdmc,  input_string,  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 

stored_ratio [i] [ j ] =atof (buffer) ; 

} 

k++; 

} 

} 

/////////////////////////////////////////////////////////////////////////// 
//  Compute  the  average  of  one  hundred  queries  to  the  current  voltage.  This 
//  is  neccessary  due  to  the  voltage  fluctuations  caused  by  noise.  Improper 
//  values  could  cause  initilization  to  be  slightly  off. 
/////////////////////////////////////////////////////////////////////////// 
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void  averagePotPower (double  *pot,  double  *potpower) 

{ 

double  pott [NUM_JOINTS] ,  temp[7],  power; 
int  i,  j  ; 

power  =  0; 


for(i=0;  i<7;  i++) 

{ 

if  (i<6) 

pott [i] 

if  ( i<7 ) 


} 


temp [ i ] 


0; 


0; 


for  ( 1=0 ;  i<100;  i++) 

{ 

for  ( j  =1 ;  j<8 ;  j++) 

{ 

sprintf ( input_string,  "AN_%d=@AN [ %d] " , j  ,  j  )  ; 

rc  =  DMCCommand (hdmc,  input_string,  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 

sprintf ( input_string,  "AN_%d=?",  j); 

rc  =  DMCCommand (hdmc,  input_string,  buffer,  sizeof (buffer) ) ; 
if  (rc) 

PrintError (rc) ; 
temp [ j -1 ] =atof (buffer) ; 
if ( j  <7 ) 

pott [ j - 1 ] +=temp [ j - 1 ] ; 

else 

power  +=  temp[j-l]; 


for  ( j  =0 ;  j<NUM_JOINTS;  j++) 

{ 

pot [ j ] =pott [ j ] /100; 


*potpower=power / 100 ; 


void  PrintError ( long  rc) 

{ 

fflush (stdout) ; 

} 


///////////////////////////////////////////////////////////////////////////////////////// 

//  File:  galillnterface .h 

//  Version:  0.1  Original  Creation 

//  Written  by:  Ognjen  Sosa  (ognjensosa@hotmail.com) 

//  Date:  09/28/2004 

// 

//  Description:  This  file  contains  function  prototypes  required  for  galillnterface . c 

///////////////////////////////////////////////////////////////////////////////////////// 


#ifndef  GAL I L I NTERFACE_H_ 

#def ine  GAL I L I NTERFACE_H_ 

#def ine  NUM_JOINTS  6 

#def ine  NUM_INDEX  70 

#def ine  INTERFACE_THREAD_TIMEOUT_SEC  1.0 

//public 

int  interfaceStartup (void)  ; 
int  interf aceShutdown (void)  ; 
void  *interfaceThread (void  *threadData) ; 
double  getlnterfaceUpdateRate (void)  ; 
int  getlnterf aceThreadRunning ( void) ; 
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//private  accessors 
int  setEf fort (double  *); 
double  get JointCmdEf fort (int)  ; 
double  get JointRealEf fort (int)  ; 

double  get JointPosition (int)  ; 
double  get JointVelocity (int)  ; 
double  get JointForceTorque (int)  ; 

int  getGalilReady (void)  ; 
int  getValReady (void)  ; 
int  getArmReady ( void)  ; 
int  getlnitialize (void)  ; 
int  getPumaHome  (void)  ; 
int  getlnterfaceReady (void)  ; 

double  getTimeSeconds (void) ; 

//private 

int  resetGalil (void) ; 
void  startGALILController (void) ; 
void  startVALController (void)  ; 
void  startARM ( void)  ; 
void  initManipulator ( long  *); 
void  homeManipulator (void)  ; 
void  setMaxSpeed (double  *); 
void  setMaxAcceleration (double  *); 
void  setMaxDeceleration (double  *); 
void  motion (double  *,  double  *); 

void  position (double  *,  double  *, double  *); 
void  velocity (double  *); 
void  forcetorque (double  *); 

void  averagePotPower (double  *,  double  *); 
void  collectRatios (double  stored_ratio [6] [70] ) ; 
void  PrintError ( long  rc)  ; 

#endif 


B.2  The  Pm.c  File  and  the  Corresponding  Header  Pm.h 


///////////////////////////////////////////////////////////////////////////////////////// 

//  File:  pm.c 

//  Version:  0.1  Original  Creation 

//  Written  by:  Ognjen  Sosa  (ognjensosa@hotmail.com) 

Template  by:  Tom  Galluzzo  (galluzzt@ufl.edu) 

Date:  09/28/2004 


// 

// 

// 

//  Description: 
// 

// 

// 


This  file  contains  the  C  code  for  implementation  of  a  Primitive 
Manipulator  JAUS  component  in  a  Linux  environment.  This  code  is 
designed  to  work  with  the  node  manager  and  JAUS  library  software 
written  by  Jeff  Witt. 


///////////////////////////////////////////////////////////////////////////////////////// 


//  JAUS  message  set  (USER:  JAUS  libraries 


//  Multi-threading  functions  (standard  to  unix) 
//  Precise  timing  routines  (USER:  must  link 


#include  <jaus.h> 
must  be  installed  first) 

#include  <pthread.h> 

#include  <timeLib.h> 
timeLib.c,  written  by  Tom  Galluzzo) 

#include  <unistd.h>  //  Unix  standard  functions 

#include  <stdio.h>  //  Unix  standard  input-output 

#include  <math.h>  //  Mathematical  functions 

#include  Cnodemgr/nodemgr . h>  //  Node  managment  functions  for  sending  and  receiving  JAUS 
messages  (USER:  Node  Manager  must  be  installed) 


#include  "pm.h" 

accessor  defintions  used  in  this  component 


//  File  containing  all  function  and 


118 


#include  "galillnterface . h"  //  Access  to  setEffortO  and  get JointPosition  ( ) 

functions 

#include  "logLib.h"  //  Debug  definitions 

#include  "mcConstants . h" 

#define  DATASIZE  JDATASIZE  //  Default  size  for  byte  stream  buffers  in  this  file 

//  Private  function  prototypes 
void  *pmStateThread (void  *) ; 
void  *pmNodemgrThread (void  *) ; 

//  State  messages  set  by  this  component 
void  pmStandbySsc (void)  ; 
void  pmResumeSsc (void)  ; 

//  Accessor  to  incoming  information 

jointEf fort_t  *  pmSetJointEf fort (void) ;  //  Accessor  to  the  value  of 

joint  efforts 

//  Accessors  to  outgoing  information 
jointEf fort_t  *  pmReport JointEf fort (void) ; 

manipulatorSpecif ications_t  *  pmReportManipulatorSpecif ications (void) ; 

unsigned  char  pmlnstld,  pmNodeld,  pmSubsId;  //  JAUS  Instance,  Node  and 

Subsystem  IDs  for  this  component 

componentAuthority_t  pmAuthority  =0;  //  JAUS  Authority  for  this 

component 

int  pmState  =  S HUT DOWN_S TATE ;  //  JAUS  State,  running 

State,  and  a  thread  running  count 

double  pmThreadHz;  // 

Stores  the  calculated  update  rate  for  main  state  thread 

unsigned  char  pmCtrlCmptlnstld,  pmCtrlCmptCompId;  //  Stores  identity  of  the  component 
currently  in  control 

unsigned  char  pmCtrlCmptNodeld,  pmCtrlCmptSubsId;  //  Stores  identity  of  the  component 
currently  in  control 

unsigned  char  pmCtrlCmptAuthority;  //  Stores  the 

authority  level  of  the  component  currently  in  control 

int  pmUnderControl  =  FALSE;  //  FALSE  -  not 

under  control,  TRUE  is  under  control 
int  pmRun  =  FALSE; 

int  pmStateThreadRunning  =  FALSE; 
pthread_t  pmStateThreadld; 
int  pmNodemgrThreadRunning  =  FALSE; 
pthread_t  pmNodemgrThreadld; 

//  Structure  storing  incoming  request  under  case  SET_JOINT  EFFORT 

static  jointEf fort_t  set JointEf fort ;  //  Structure  defining  data  received  and  sent 

by  this  component 

//  Structure  storing  incoming  request  under  case  QUERY  JOINT  EFFORT 
static  jointEf fort_t  reportedJointEf fort ; 

//  Structure  storing  incoming  request  under  case  QUERY  MANIPULATOR  SPECIFICATIONS 
static  manipulatorSpecif i cat ions_t  reportedManipulatorSpecif ications; 

jms_t  pmJms;  //  An  accessor  to  the  Node  Manager  JAUS  Message  Service  (Jms)  for  this 
component 

//  Function:  pmStartup 

//  Access:  Public 

//  Description:  This  function  allows  the  abstracted  component  functionality  contained  in 
this  file  to  be  started  from  an  external  source. 

//  It  must  be  called  first  before  the  component  state  machine 

and  node  manager  interaction  will  begin 

//  Each  call  to  "cmptStartup"  should  be  followed  by  one  call 

to  the  "cmptShutdown"  function 

int  pmStartup (void) 

{ 

pthread_attr_t  attr;  //  Thread  attributed  for  the  component  threads  spawned  in 
this  function 


119 


if (pmState  ==  SHUTDOWN_STATE)  //  Execute  the  startup  routines  only  if  the 
component  is  not  running 
{ 

//  Check  in  to  the  Node  Manager  and  obtain  Instance,  Node  and  Subsystem 

IDs 

if ( jcmCheckln (PRIMITIVE_MANIPULATOR,  Spmlnstld,  SpmNodeld,  SpmSubsId)  <  0) 
//  USER:  Insert  your  component  ID  define  on  this  line 


cError ("pm:  Could  not  check  in  to  nodemgr\nn); 
return  PM  NODE  MANAGER  CHECKIN  ERROR; 


//  Open  a  conection  to  the  Node  Manager  and  obtain  a  Jms  accessor 
if ( (pmJms  =  jmsOpen (PRIMITIVE_MANIPULATOR,  pmlnstld,  0))  ==  NULL)  //  USER: 
Insert  your  component  ID  define  on  this  line 


component 


j  cmCheckOut ( PRIMITIVE_MANIPULATOR, 
ID  define  on  this  line 


return  PM  JMS  OPEN  ERROR; 


pmlnstld) ; 


//  USER:  Insert  your 


pmJms->timeout . tv_usec  =  100000;  //  Timeout  for  non-blocking  jmsRecv, 
specified  in  micro  seconds 

pthread_attr_init (&attr)  ; 

pthread_attr_setdetachstate (&attr,  PTHREAD_CREATE_DETACHED) ; 

//  Set  the  state  of  the  JAUS  state  machine  to  INITIALIZE 
pmState  =  INITIALIZE_STATE; 
pmRun  =  TRUE; 

if (pthread_create ( &pmStateThreadId,  &attr,  pmStateThread,  NULL)  !=  0) 

{ 

cError ("pm:  Could  not  create  cmptStateThread\n" ) ; 
pmShutdown ( ) ; 

return  PM_STATE_THREAD_CREATE_ERROR; 

} 

if (pthread_create ( &pmNodemgrThreadId,  &attr,  pmNodemgrThread,  NULL)  !=  0) 

{ 

cError ("pm:  Could  not  create  cmptNodemgrThread\n" ) ; 
pmShutdown ( ) ; 

return  PM_NODEMGR_THREAD_CREATE_ERROR; 

} 

pthread_attr_destroy ( &attr)  ; 

} 

else 


cError ("pm:  Attempted  startup  while  not  shutdown\n" ) ; 
return  PM  STARTUP  BEFORE  SHUTDOWN  ERROR; 


return 


0; 


//  Function:  pmShutdown 
//  Access:  Public 

//  Description:  This  function  allows  the  abstracted  component  functionality 

contained  in  this  file  to  be  stoped  from  an  external  source. 

//  If  the  component  is  in  the  running  state,  this  function 

will  terminate  all  threads  running  in  this  file 

//  This  function  will  also  close  the  Jms  connection  to  the 

Node  Manager  and  check  out  the  component  from  the  Node  Manager 

int  pmShutdown (void) 

{ 

double  timeOutSec; 
double  effort [NUM_JOINTS] ; 
int  i; 
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if  (pmState  !=  SHUTDOWN_STATE)  //  Execute  the  startup  routines  only  if  the 
component  is  not  running 
{ 


pmRun  =  FALSE; 


timeOutSec  =  getTimeSeconds ( )  +  PM_THREAD_TIMEOUT_SEC; 
while (pmStateThreadRunning) 

{ 

usleep (100000) ; 

if (getTimeSeconds ( )  >=  timeOutSec) 

{ 

pthread_cancel (pmStateThreadld) ; 
pmStateThreadRunning  =  FALSE; 

cError ("pm:  cmptStateThread  Shutdown  Improperly\n" ) ; 
break; 

} 


timeOutSec  =  getTimeSeconds ( )  +  PM_THREAD_TIMEOUT_SEC; 
while (pmNodemgrThreadRunning) 

{ 

usleep (100000) ; 

if (getTimeSeconds ( )  >=  timeOutSec) 

{ 

pthread_cancel (pmNodemgrThreadld) ; 
pmNodemgrThreadRunning  =  FALSE; 

cError ("pm:  cmptNodemgrThread  Shutdown  Improperly\n" ) ; 
break; 


//  Set  jog  values  to  zero  eliminating  possibility  of  jerky  motion 
for  ( i=0 ;  i<NUM_JOINTS ;  i++) 

{ 

effort [i]  =  0; 

} 

setEffort (effort) ; 

//  Set  the  state  of  the  subsystem  commander  to  standby 
pmStandbySsc  ( ) ; 

//  Close  Node  Manager  Connection  and  check  out 
jmsClose (pmjms) ; 

jcmCheckOut (PRIMITIVE_MANIPULATOR,  pmlnstld) ;  //  USER:  Insert  your 
component  ID  define  on  this  line 

pmState  =  SHUTDOWN_STATE; 

} 


return  0; 

} 

//  The  series  of  functions  below  allow  public  access  to  essential  component  information 

//  Access:  Public  (All) 

int  pmGetState (void) {  return  pmState;  } 

unsigned  char  pmGetlnstanceld ( void) {  return  pmlnstld;  } 

unsigned  char  pmGetCompId ( void) {  return  (unsigned  char ) PRIMITIVE_MANIPULATOR;  } 
unsigned  char  pmGetNodeld ( void) {  return  pmNodeld;  } 
unsigned  char  pmGetSubsystemld ( void) {  return  pmSubsId;  } 
double  pmGetUpdateRate (void) {  return  pmThreadHz;  } 

jointEf fort_t  *  pmSetJointEf fort (void)  {  return  &set JointEf fort;  } 
jointEf fort_t  *  pmReport JointEf fort (void)  {  return  &reportedJointEf fort;  } 
manipulatorSpecif ications_t  *  pmReportManipulatorSpecif ications (void)  {  return 
&reportedManipulator Specifications;  } 

//  Function:  pmThread 
//  Access:  Private 

//  Description:  All  core  component  functionality  is  contained  in  this  thread. 

//  All  of  the  JAUS  component  state  machine  code  can  be  found 

here . 

void  *pmStateThread (void  *threadData) 
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jmh_t  txMsgHeader; 

unsigned  char  data [DATASIZE] ; 

double  time,  prevTime,  effort [NUM_JOINTS] ,  curr_position [NUM_JOINTS] ; 
int  i; 


pmStateThreadRunning  =  TRUE;  // 

//  Setup  transmit  header 
txMsgHeader . isExpMsg  =  0; 
if  experimental 

txMsgHeader . isSvcMsg  =  0; 
txMsgHeader . acknCtrl  =  0; 
txMsgHeader . priority  =  6; 
txMsgHeader . reserved  =  0; 
txMsgHeader . version  =  1; 
txMsgHeader . srclnstld  = 
txMsgHeader . srcCompId  = 
txMsgHeader . srcNodeld  =  pmNodeld; 
txMsgHeader . srcSubsId  =  pmSubsId; 
txMsgHeader . dataCtrl  =  0; 
txMsgHeader . seqNumber  =  0; 


//  not  experimental  msg  -  USER:  change  to  1 

//  not  a  service  connection 
//  no  ack/nak 
//  default  priority 


//  JAUS  3.0 

pmlnstld; 

PRIMITIVE  MANIPULATOR; 


time  =  getTimeSeconds ()  ; 

pmState  =  INITIALIZE_STATE;  //  Set  JAUS  state  to  INITIALIZE 


while (pmRun)  //  Execute  state  machine  code  while  not  in  the  SHUTDOWN  state 

{ 

prevTime  =  time; 

time  =  getTimeSeconds () ; 

pmThreadHz  =  1 . 0/ (time-prevTime) ;  //  Compute  the  update  rate  of  this 

thread 

switch (pmState)  //  Switch  component  behavior  based  on  which  state  the 

machine  is  in 


case  INITIALIZE_STATE: 

//  Check  for  system  initialization 
if (getlnterfaceReady ( ) )  { 

pmState  =  READY_STATE ; 


else 

pmState  =  STANDBY_STATE; 

break; 


case  STANDBY_STATE : 
break; 


case  READY_STATE: 

//  Resume  system  commander  if  in  standby  mode 
pmResumeSsc ( ) ; 

//  Command  effort  to  the  manipulator  interface 
reportedJointEf fort . numJoints  =  NUM_JOINTS; 

for (i=0 ;  i<NUM_JOINTS;  i++) 

{ 

reportedJointEf fort . jointEf fort [i]  = 

get JointRealEf fort (i)  ; 

effort [i]  =  set JointEf fort . jointEf fort [i] ; 


setEffort (effort) ; 


//  Set  up  fail-safe  with  software  limits  a  little  lower 

than  mechanical  limits 

for  (i=0;  i<NUM_JOINTS;  i++) 

{ 

curr_position [i]  =  get JointPosition (i) ; 
if  (i  <  5)  { 

if  (fabs (curr_position [i] )  > 

reportedManipulatorSpecif ications . j ointSpecif ications [i] . j ointMaximumValue  /  1000) 

pmState  =  EMERGENCY_STATE ; 


122 


} 

else 

if  (fabs (curr_position [i] )  > 

reportedManipulatorSpecif ications . j ointNmaximumValue  /  1000) 

pmState  =  EMERGENCY_STATE; 


} 

break; 

case  EMERGENCY_STATE: 

//  If  emergency  occurs,  command  zero  effort 
for  (1=0;  i<NUM_JOINTS;  i++) 

{ 

effort [i]  =  0; 

} 

setEffort (effort) ; 
break; 

case  FAILURE_STATE; 

//  If  failure  occurs,  command  zero  effort 
for  (1=0;  i<NUM_JOINTS;  i++) 

{ 

effort [i]  =  0; 

} 

setEffort (effort) ; 
break; 

case  SHUTDOWN_STATE: 
break; 

default : 

pmState  =  FAILURE_STATE;  //  The  default  case  is  undefined, 
therefore  go  into  Failure  State 

break; 

} 

usleep (1000) ;  //  Sleep  for  one  millisecond 


if (pmUnderControl ) 

{ 

//  Terminate  control  of  current  component 
txMsgHeader . cmdCode  =  REJECT_COMPONENT_CONTROL; 

txMsgHeader . dstSubsId  =  pmCtrlCmptSubsId; 
txMsgHeader . dstNodeld  =  pmCtrlCmptNodeld; 
txMsgHeader . dstCompId  =  pmCtrlCmptCompId; 
txMsgHeader . dstlnstld  =  pmCtrlCmptlnstld; 
txMsgHeader . dataBytes  =  0; 

jmsSend (pmjms,  & txMsgHeader ,  data,  txMsgHeader .dataBytes); 


//  Sleep  for  50  milliseconds  and  then  exit 
usleep (50000) ; 

pmStateThreadRunning  =  FALSE; 
pthread_exit (NULL) ; 

} 


//  Function:  cmptNodemgrThread 
//  Access:  Private 

//  Description:  This  thread  is  responsible  for  handling  incoming  JAUS  messages  from 

the  Node  Manager  JAUS  message  service  (Jms) 

//  incoming  messages  are  processed  according  to  message  type, 

void  *pmNodemgrThread ( void  *threadData) 

{ 

jmh_t  rxMsgHeader,  txMsgHeader;  //  Declare  JAUS  header  data  structures,  where  the 
jmh_t  data  structure  is  defined  in  jaus.h 
unsigned  char  data [DATASIZE] ; 

createServiceConnection_t  createServiceConnection; 
conf irmServiceConnection_t  conf irmServiceConnection; 
activateServiceConnection  t  activateServiceConnection; 
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suspendServiceConnection_t  suspendServiceConnection; 
terminateServiceConnection_t  terminateServiceConnection; 
requestComponentControl_t  requestComponentControl ; 
conf irmComponentControl_t  conf irmComponentControl; 
component Author ity_t  componentAuthority; 
componentStatus_t  componentStatus ; 
int  recvCount; 

pmNodemgrThreadRunning  =  TRUE;  //  Increment  the  number  of  threads  running  in  this 

file 

//  Setup  Manipulator  Specifications 

reportedManipulatorSpecif ications . numJoints  =  NUM_JOINTS; 
reportedManipulatorSpecif ications . jointNtype  =  1; 

reportedManipulatorSpecif ications . j ointNof f setOrAngle  =  129;  //mm 
reportedManipulatorSpecif ications . j ointNminimumValue  =  0  /  J6_RAD_T0_ENC  *  1000; 
//10A-3  rad 

reportedManipulatorSpecif ications . j ointNmaximumValue  =  31000  /  J6_RAD_TO_ENC  * 
1000;  //10 A-3  rad 

reportedManipulatorSpecif ications . j ointNmaximumVelocity  =  1000  /  J6_RAD_TO_ENC  * 
1000;//10A-3  rad/sec 

reportedManipulatorSpecif ications .manipulatorCoordinateSystemX  =  0 ; 
reportedManipulatorSpecif ications .manipulatorCoordinateSystemY  =  0 ; 
reportedManipulatorSpecif ications .manipulatorCoordinateSystemZ  =  0 ; 
reportedManipulatorSpecif ications . quaternionQcomponentD  =  1; 
reportedManipulatorSpecif ications . quaternionQcomponentA  =  0; 
reportedManipulatorSpecif ications . quaternionQcomponentB  =  0; 
reportedManipulatorSpecif ications . quaternionQcomponentC  =  0; 

reportedManipulatorSpecif ications . j ointSpecif ications [0] .jointType  =  1; 
reportedManipulatorSpecif ications . j ointSpecif ications [ 0 ]. linkLength  =  0.0;  //mm 
reportedManipulatorSpecif ications . j ointSpecif ications [0] . twistAngle  =  90  * 
DEG_TO_RAD  *  1000;  //10A-3  rad 

reportedManipulatorSpecif ications . j ointSpecif ications [0] . j ointOff setOrAngle  =  0; 

//mm 

reportedManipulatorSpecif ications . j ointSpecif ications [ 0 ]. j ointMinimumValue  =  0  / 
Jl_RAD_TO_ENC  *  1000;  //10A-3  rad 

reportedManipulatorSpecif ications . j  ointSpecif ications [ 0 ]  . j  ointMaximumValue  = 
224000  /  Jl_RAD_TO_ENC  *  1000;  //10A-3  rad 

reportedManipulatorSpecif ications . j  ointSpecif ications [ 0 ]  . j  ointMaximumVelocity  = 
5000  /  Jl_RAD_TO_ENC  *  1000;  //10A-3  rad/ sec 

reportedManipulatorSpecif ications . j ointSpecif ications [1] .jointType  =  1; 
reportedManipulatorSpecif ications . j ointSpecif ications [ 1 ]. linkLength  =  650;  //mm 
reportedManipulatorSpecif ications . j ointSpecif ications [1] . twistAngle  =  0;  //10A-3 

rad 

reportedManipulatorSpecif ications . j  ointSpecif ications [ 1 ]  . j  ointOff setOrAngle  = 

190 ; //mm 

reportedManipulatorSpecif ications . j ointSpecif ications [ 1 ]. j ointMinimumValue  =  0  / 
J2_RAD_TO_ENC  *  1000;  //10A-3  rad 

reportedManipulatorSpecif ications . j  ointSpecif ications [ 1 ]  . j  ointMaximumValue  = 
224000  /  J2_RAD_TO_ENC  *  1000;  //10A-3  rad 

reportedManipulatorSpecif ications . j  ointSpecif ications [ 1 ]  . j  ointMaximumVelocity  = 
5000  /  J2_RAD_TO_ENC  *  1000;  //10A-3  rad/ sec 

reportedManipulatorSpecif ications . j ointSpecif ications [2] .jointType  =  1; 
reportedManipulatorSpecif ications . j ointSpecif ications [2 ] .linkLength  =  0.0; //mm 
reportedManipulatorSpecif ications . j ointSpecif ications [2 ]. twistAngle  =  270  * 
DEG_TO_RAD  *  1000;  //10A-3  rad 

reportedManipulatorSpecif ications . j ointSpecif ications [2 ] . j ointOff setOrAngle  =  0; 

//mm 

reportedManipulatorSpecif ications . j ointSpecif ications [2 ]. j ointMinimumValue  =  0  / 
J3_RAD_TO_ENC  *  1000;  //10A-3  rad 

reportedManipulatorSpecif ications . j  ointSpecif ications [2 ]  . j  ointMaximumValue  = 
224000  /  J3_RAD_TO_ENC  *  1000;//10A-3  rad 

reportedManipulatorSpecif ications . j  ointSpecif ications [2 ]  . j  ointMaximumVelocity  = 
5000  /  J3_RAD_TO_ENC  *  1000;  //10A-3  rad/ sec 

reportedManipulatorSpecif ications . j ointSpecif ications [3] .jointType  =  1; 
reportedManipulatorSpecif ications . j ointSpecif ications [3] . linkLength  =  0.0;  //mm 
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reportedManipulatorSpecif ications . j ointSpecif ications [3] 
DEG_TO_RAD  *  1000;  //10A-3  rad 

reportedManipulatorSpecif ications . j ointSpecif ications [3] 
//mm 

reportedManipulatorSpecif ications . j ointSpecif ications [3] 
J4_RAD_TO_ENC  *  1000;  //10A-3  rad 

reportedManipulatorSpecif ications . j ointSpecif ications [3] 
/  J4_RAD_TO_ENC  *  1000;  //10A-3  rad 

reportedManipulatorSpecif ications . j ointSpecif ications [3] 
1000  /  J4  RAD  TO  ENC  *  1000;  //10A-3  rad/sec 


.twistAngle  =  90  * 

.  jointOf fsetOrAngle  =  600; 
.  j ointMinimumValue  =  0  / 

.  j ointMaximumValue  =  70000 
.  j ointMaximumVelocity  = 


reportedManipulatorSpecif ications . j ointSpecif ications [4] 
reportedManipulatorSpecif ications . j ointSpecif ications [4] 
reportedManipulatorSpecif ications . j ointSpecif ications [4] 
DEG_TO_RAD  *  1000;  //10A-3  rad 

reportedManipulatorSpecif ications . j ointSpecif ications [4] 

//mm 

reportedManipulatorSpecif ications . j ointSpecif ications [4] 
J5_RAD_TO_ENC  *  1000;  //10A-3  rad 

reportedManipulatorSpecif ications . j ointSpecif ications [4] 
/  J5_RAD_TO_ENC  *  1000;  //10A-3  rad 

reportedManipulatorSpecif ications . j ointSpecif ications [4] 
1000  /  J5  RAD  TO  ENC  *  1000;  //10A-3  rad/sec 


.jointType  =  1; 

.linkLength  =  0.0;  //mm 
.twistAngle  =  90  * 

. j ointOf fsetOrAngle  =  0; 

.  j ointMinimumValue  =  0  / 

.  j ointMaximumValue  =  32000 

.  j ointMaximumVelocity  = 


//  Setup  transmit  header 
txMsgHeader . isExpMsg  =  0; 
if  experimental 

txMsgHeader . isSvcMsg  =  0; 
txMsgHeader . acknCtrl  =  0; 
txMsgHeader . priority  =  6; 
txMsgHeader . reserved  =  0; 
txMsgHeader . version  =  1; 
txMsgHeader . srclnstld  =  pmlnstld; 
txMsgHeader . srcCompId  =  PRIMITIVE 
txMsgHeader . srcNodeld  =  pmNodeld; 
txMsgHeader . srcSubsId  =  pmSubsId; 
txMsgHeader . dataCtrl  =  0; 
txMsgHeader . seqNumber  =  0; 


//  not  experimental  msg  -  USER:  change  to  1 

//  not  a  service  connection 
//  no  ack/nak 
//  default  priority 

//  JAUS  3.0 

MANIPULATOR; 


while (pmRun)  //  Execute  incoming  JAUS  message  processing  while  not  in  the 
SHUTDOWN  state 
{ 

//  Check  for  a  new  message  in  the  Node  Manager  JMS  queue 
//  If  a  message  is  present  then  store  the  header  information  in 
rxMsgHeader,  and  store  the  message  content  information  in  the  data  buffer 
recvCount  =  jmsRecv (pmJms,  SrxMsgHeader ,  data,  DATASIZE) ; 
if  (recvCount  <  0) 

{ 

cError ("pm:  Node  manager  jmsRecv  returned  error:  %d\n",  recvCount); 
break; 


if  (recvCount  ==  0)  continue; 


//  This  block  of  code  is  intended  to  reject  commands  from  non-controlling 

components 

//  However,  the  one  exception  allowed  is  a  REQUEST_COMPONENT_CONTROL 
if (pmUnderControl  &&  rxMsgHeader . cmdCode  <  0x2000)  //  If  not  currently 
under  control  or  it's  not  a  command,  then  go  to  the  switch  statement 
{ 


//  If  the  source  component  isn't  the  one  in  control,  and  it's  not  a 
request  to  gain  control,  then  exit  this  iteration  of  the  while  loop 

if ( ! (pmCtrlCmptInstId==rxMsgHeader . srclnstld  && 

pmCtrlCmptCompId==rxMsgHeader . srcCompId  && 
pmCtrlCmptNodeId==rxMsgHeader . srcNodeld  && 
pmCtrlCmptSubsId==rxMsgHeader . srcSubsId) && 

! ( rxMsgHeader . cmdCode==REQUEST_COMPONENT_CONTROL ) 

) continue;  //  to  next  iteration 

} 


switch (rxMsgHeader . cmdCode)  //  Switch  the  processing  algorithm  according 
to  the  JAUS  message  type 
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code 

UNPACK) ; 


//  Set  the  component  authority  according  to  the  incoming  authority 

case  SET_COMPONENT_AUTHORITY : 

convertComponentAuthority (data,  &pmAuthority,  DATASIZE, 

//  Unpack  and  store  the  incoming  authority  code 
break; 

case  SHUTDOWN: 

pmstate  =  S HUT DOWN_S TATE ; 
break; 

case  STANDBY: 

if (pmstate  ==  READY_STATE) 

pmstate  =  STANDBY_STATE; 

break; 

case  RESUME: 

if (pmstate  ==  STANDBY_STATE ) 

pmstate  =  READY_STATE ; 

break; 
case  RESET: 

pmstate  =  INITIALIZE_STATE; 
break; 

case  SET_EMERGENCY : 

pmstate  =  EMERGENCY_STATE; 
break; 

case  CLEAR_EMERGENCY : 

pmstate  =  STANDBY_STATE; 
break; 


case  CREATE_SERVICE_CONNECTION: 

convertCreateServiceConnection (data, 
ScreateServiceConnection,  DATASIZE,  UNPACK) ; 

//  USER:  Insert  code  here  to  handle  the  create  service 


connection  message 


break; 


case  CONFIRM_SERVICE_CONNECTION : 

convertConf irmServiceConnection (data, 

&conf irmServiceConnection,  DATASIZE,  UNPACK) ; 

//  USER:  Insert  code  here  to  handle  the  confirm  service 


connection  message 


break; 


case 

SactivateServiceConnection, 
connection  message 


ACT I VATE_SERVI CE_CONNECT I ON : 

convertActivateServiceConnection (data, 

DATASIZE,  UNPACK) ; 

//  USER:  Insert  code  here  to  handle  the  activate  service 
break; 


case  SUSPEND_SERVICE_CONNECTION: 

convert SuspendServiceConnect ion (data, 
SsuspendServiceConnection,  DATASIZE,  UNPACK) ; 

//  USER:  Insert  code  here  to  handle  the  suspend  service 


connection  message 


break; 


case  TERMINATE_SERVICE_CONNECTION : 

convertTerminateServiceConnection (data, 
SterminateServiceConnection,  DATASIZE,  UNPACK) ; 

//  USER:  Insert  code  here  to  handle  the  terminate  service 


connection  message 


break; 


case  REQUEST_COMPONENT_CONTROL: 
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convertRequestComponentControl (data, 
SrequestComponentControl,  DATASIZE,  UNPACK) ; 

if (pmUnderControl) 


Test  for  higher  authority 


if (requestComponentControl  >  pmCtrlCmptAuthority)  // 


REJECT  COMPONENT  CONTROL; 


txMsgHeader . dataBytes)  ; 


//  Terminate  control  of  current  component 
txMsgHeader . cmdCode  = 

txMsgHeader . dstSubsId  =  pmCtrlCmptSubsId; 
txMsgHeader . dstNodeld  =  pmCtrlCmptNodeld; 
txMsgHeader . dstCompId  =  pmCtrlCmptCompId; 
txMsgHeader . dstlnstld  =  pmCtrlCmptlnstld; 
txMsgHeader . dataBytes  =  0; 
jmsSend (pmJms,  StxMsgHeader ,  data. 


CONFIRM_COMPONENT_CONTROL; 
rxMsgHeader . srcSubsId; 
rxMsgHeader . srcNodeld; 
rxMsgHeader . srcCompId; 
rxMsgHeader . srclnstld; 

convertConf irmComponentControl (data,  &conf j 
txMsgHeader . dataBytes)  ; 

requestComponentControl ; 


//  Accept  control  of  new  component 
txMsgHeader . cmdCode  = 

txMsgHeader . dstSubsId  = 

txMsgHeader . dstNodeld  = 

txMsgHeader . dstCompId  = 

txMsgHeader . dstlnstld  = 

conf irmComponentControl . asByte  =  0; 
txMsgHeader . dataBytes  = 
rmComponentControl,  DATASIZE,  PACK) ; 
jmsSend (pmJms,  StxMsgHeader ,  data, 

pmCtrlCmptlnstld  =  rxMsgHeader . srclnstld; 
pmCtrlCmptCompId  =  rxMsgHeader . srcCompId; 
pmCtrlCmptNodeld  =  rxMsgHeader . srcNodeld; 
pmCtrlCmptSubsId  =  rxMsgHeader . srcSubsId; 
pmCtrlCmptAuthority  = 


else 


if  (  rxMsgHeader . srcSubsId  !  = 

pmCtrlCmptSubsId  | |  rxMsgHeader . srcNodeld  !=  pmCtrlCmptNodeld  | | 

rxMsgHeader . srcCompId  != 

pmCtrlCmptCompId  | |  rxMsgHeader . srclnstld  !=  pmCtrlCmptlnstld  ) 


RE JECT_COMPONENT_CONTROL; 
rxMsgHeader . srcSubsId; 
rxMsgHeader . srcNodeld; 
rxMsgHeader . srcCompId; 
rxMsgHeader . srclnstld; 

txMsgHeader . dataBytes)  ; 


txMsgHeader . cmdCode  = 

txMsgHeader . dstSubsId  = 

txMsgHeader . dstNodeld  = 

txMsgHeader . dstCompId  = 

txMsgHeader . dstlnstld  = 

txMsgHeader . dataBytes  =  0; 
jmsSend (pmJms,  StxMsgHeader ,  data. 


} 

} 

else  //  Not  currently  under  component  control,  so  give 

control 


txMsgHeader . cmdCode  =  CONFIRM_COMPONENT_CONTROL; 
txMsgHeader . dstSubsId  =  rxMsgHeader . srcSubsId; 
txMsgHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
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convertConf irmComponentControl (data, 
txMsgHeader . dataBytes)  ; 


} 

break; 


txMsgHeader . dstCompId  =  rxMsgHeader . srcCompId; 
txMsgHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
conf irmComponentControl . asByte  =  0; 
txMsgHeader . dataBytes  = 

&conf irmComponentControl,  DATASIZE,  PACK) ; 
jmsSend (pmJms,  StxMsgHeader ,  data, 

pmCtrlCmptlnstld  =  rxMsgHeader . srclnstld; 
pmCtrlCmptCompId  =  rxMsgHeader . srcCompId; 
pmCtrlCmptNodeld  =  rxMsgHeader . srcNodeld; 
pmCtrlCmptSubsId  =  rxMsgHeader . srcSubsId; 
pmCtrlCmptAuthority  =  requestComponentControl; 
pmUnderControl  =  TRUE; 


case  RELEASE_COMPONENT_CONTROL : 

pmUnderControl  =  FALSE; 
break; 


case  CONFIRM_COMPONENT_CONTROL: 

convertConf irmComponentControl (data, 

&conf irmComponentControl,  DATASIZE,  UNPACK) ; 

//  USER:  Insert  code  here  to  handle  the  confirm  component 

control  message  if  needed 


break; 


case 

control  message  if  needed 


RE JECT_COMPONENT_CONTROL : 

//  USER:  Insert  code  here  to  handle  the  reject  component 
break; 


case  QUERY_COMPONENT_AUTHORITY: 

txMsgHeader . cmdCode  =  RE  PORT_COMPONENT_AUTHORI T Y ; 

txMsgHeader . dstSubsId  =  rxMsgHeader . srcSubsId; 
txMsgHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
txMsgHeader . dstCompId  =  rxMsgHeader . srcCompId; 
txMsgHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
componentAuthority  =  pmAuthority; 

txMsgHeader . dataBytes  =  convertComponentAuthority (data, 
ScomponentAuthority,  DATASIZE,  PACK) ; 

jmsSend (pmJms,  StxMsgHeader ,  data,  txMsgHeader . dataBytes) 
break; 


case  QUERY_COMPONENT_STATUS : 

txMsgHeader . cmdCode  =  REPORT_COMPONENT_STATUS ; 

txMsgHeader . dstSubsId  =  rxMsgHeader . srcSubsId; 
txMsgHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
txMsgHeader . dstCompId  =  rxMsgHeader . srcCompId; 
txMsgHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
componentStatus . primaryStatus . asByte  =  pmState; 
txMsgHeader . dataBytes  =  convertComponentStatus (data, 
ScomponentStatus ,  DATASIZE,  PACK) ; 

jmsSend (pmJms,  StxMsgHeader ,  data,  txMsgHeader . dataBytes) 
break; 


DATASIZE,  UNPACK) ; 
authority  message  if 


case  REPORT_COMPONENT_AUTHORITY: 

convertComponentAuthority (data,  ScomponentAuthority, 

//  USER:  Insert  code  here  to  handle  the  report  component 

needed 

break; 


UNPACK) ; 


case  RE PORT_COMPONENT_S TATU S : 

convertComponentStatus (data,  ScomponentStatus,  DATASIZE, 

break; 


case  SET_JOINT_EFFORT: 

convert JointEf fort (data,  &set JointEf fort ,  DATASIZE, 


UNPACK) ; 
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break; 


case  QUERY_JOINT_EFFORT: 

txMsgHeader . cmdCode  =  RE  PORT_ JO I NT_E  FFORT ; 
txMsgHeader . dstlnstld 
txMsgHeader . dstCompId 
txMsgHeader . dstNodeld 
txMsgHeader . dstSubsId 
txMsgHeader . dataBytes 
&reportedJointEf f ort ,  DATASIZE,  PACK) ; 

jmsSend (pmjms,  & txMsgHeader ,  data,  txMsgHeader .dataBytes) 
break; 


rxMsgHeader . srclnstld; 
rxMsgHeader . srcCompId; 
rxMsgHeader . srcNodeld; 
rxMsgHeader . srcSubsId; 
convert JointEf fort (data. 


case  QUERY_MAN I PULATOR_S PEC I F I CAT I ON S : 

//  unpack  presence  vector 

txMsgHeader. cmdCode  =  REPORT_MANIPULATOR_SPECIFICATIONS; 
txMsgHeader . dstSubsId  =  rxMsgHeader . srcSubsId; 
txMsgHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
txMsgHeader . dstCompId  =  rxMsgHeader . srcCompId; 
txMsgHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
txMsgHeader . dataBytes  = 

convertManipulatorSpecif ications (data,  SreportedManipulatorSpecifications,  DATASIZE, 
PACK)  ; 

jmsSend (pmjms,  & txMsgHeader ,  data,  txMsgHeader .dataBytes) 
break; 

default : 

break; 


} 

pmNodemgrThreadRunning  =  FALSE; 
pthread_exit (NULL)  ; 


void  pmStandbySsc (void) 

{ 

jirnh  _t  txMsg; 

unsigned  char  data [DATASIZE] ; 
double  requestTimeOutSec  =  0.1; 
static  double  lastRequestTimeSec  =  0; 

//  Setup  Transmit  msg 

txMsg . isExpMsg  =  0; 

txMsg . priority  =  6; 

txMsg. acknCtrl  =  0; 

txMsg . isSvcMsg  =  0; 

txMsg . version  =  1; 

txMsg . reserved  =  0; 

txMsg . srclnstld  =  pmlnstld; 

txMsg. srcCompId  =  PRIMITIVE_MANIPULATOR; 

txMsg . srcNodeld  =  pmNodeld; 

txMsg. srcSubsId  =  pmSubsId; 

txMsg. dataCtrl  =  0; 

txMsg . seqNumber  =  0; 

txMsg . cmdCode  =  STANDBY; 

txMsg . dstlnstld  =  1; 

txMsg. dstCompId  =  SUBSYSTEM_COMMANDER; 
txMsg . dstNodeld  =  pmNodeld; 
txMsg. dstSubsId  =  pmSubsId  ; 
txMsg. dataBytes  =  0; 

if (  (getTimeSeconds ( )  -  lastRequestTimeSec)  >=  requestTimeOutSec) 

{ 

jmsSend (pmjms,  &txMsg,  data,  txMsg. dataBytes ) ; 
lastRequestTimeSec  =  getTimeSeconds (); 


void  pmResumeSsc (void) 
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jmh_t  txMsg; 

unsigned  char  data [DATASIZE] ; 
double  requestTimeOutSec  =  0.1; 
static  double  lastRequestTimeSec  =  0; 

//  Setup  Transmit  msg 

txMsg . isExpMsg  =  0; 

txMsg . priority  =  6; 

txMsg. acknCtrl  =  0; 

txMsg . isSvcMsg  =  0; 

txMsg . version  =  1; 

txMsg . reserved  =  0; 

txMsg . srclnstld  =  pmlnstld; 

txMsg. srcCompId  =  PRIMITIVEJXLANIPULATOR; 

txMsg. srcNodeld  =  pmNodeld; 

txMsg. srcSubsId  =  pmSubsId; 

txMsg. dataCtrl  =  0; 

txMsg . seqNumber  =  0; 

txMsg . cmdCode  =  RESUME; 

txMsg . dstlnstld  =  1; 

txMsg. dstcompld  =  SUBSYSTEM_COMMANDER; 
txMsg . dstNodeld  =  pmNodeld; 
txMsg. dstSubsId  =  pmSubsId  ; 
txMsg. dataBytes  =  0; 

if (  (getTimeSeconds ( )  -  lastRequestTimeSec)  >=  requestTimeOutSec) 

{ 

jmsSend (pmjms,  StxMsg,  data,  txMsg. dataBytes ) ; 
lastRequestTimeSec  =  getTimeSeconds () ; 

} 


///////////////////////////////////////////////////////////////////////////////////////// 

//  File:  pm.h 

//  Version:  0.1  Original  Creation 

//  Written  by:  Ognjen  Sosa  (ognjensosa@hotmail.com) 


//  Template  by: 
//  Date: 

// 

//  Description: 
// 


Tom  Galluzzo  (galluzzt@ufl.edu) 

09/28/2004 

This  file  contains  the  skeleton  C  header  file  code  for  implementing 
the  pm.c  file 

///////////////////////////////////////////////////////////////////////////////////////// 


#ifndef  PM_H 
#define  PM_H 

#include  "jaus.h" 

#ifndef  FALSE 
#def ine  FALSE  0 
#endif 

#ifndef  TRUE 
#define  TRUE  1 
#endif 


#def ine  PM_NODE_MANAGER_CHECKIN_ERROR  -1 

#def ine  PM_JMS_OPEN_ERROR 

#def ine  PM_STARTUP_BEFORE_SHUTDOWN_ERROR  -3 

#def ine  PM_STATE_THREAD_CREATE_ERROR  -4 

#def ine  PM  NODEMGR  THREAD  CREATE  ERROR  -5 


#def ine  PM  THREAD  TIMEOUT  SEC 


1.0 


//  Public 

int  pmStartup (void) ; 

int  pmShutdown (void) ; 

int  pmGetState (void)  ; 

unsigned  char  pmGetlnstanceld ( void) ; 

unsigned  char  pmGetCompId ( void) ; 

unsigned  char  pmGetNodeld ( void) ; 

unsigned  char  pmGetSubsystemld ( void) ; 
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double  pmGetUpdateRate (void) ; 

//  State  messages  set  by  this  component 
void  pmResetSsc (void)  ; 
void  pmResumeSsc (void) ; 

//  Private  accessors 

jointEf fort_t  *  pmSetJointEf fort (void) ; 
jointEf fort_t  *  pmReport JointEf fort (void) ; 

manipulatorSpecif ications_t  *  pmReportManipulatorSpecif ications (void) ; 
#endif  //  PM  H 


B.3  The  Mjps.c  File  and  the  Corresponding  Header  Mjps.h 


///////////////////////////////////////////////////////////////////////////////////////// 


// 

// 

// 

// 

// 

// 

// 

// 

// 

// 


File:  mjps.c 

Version:  0.1  Original  Creation 

Written  by:  Ognjen  Sosa  (ognjensosa@hotmail.com) 


Template  by: 
Date : 

Description : 


Tom  Galluzzo 
09/28/2004 


(galluzztQuf 1 . edu) 


This  file  contains  the  C  code  for  implementation  of  a  Manipulator 
Joint  Position  Sensor  JAUS  component  in  a  Linux  environment.  This 
code  is  designed  to  work  with  the  node  manager  and  JAUS  library 
software  written  by  Jeff  Witt. 


///////////////////////////////////////////////////////////////////////////////////////// 


#include  <jaus.h>  //  JAUS  message  set  (USER:  JAUS  libraries 

must  be  installed  first) 

#include  <pthread.h>  //  Multi-threading  functions  (standard  to  unix) 

#include  <timeLib.h>  //  Precise  timing  routines  (USER:  must  link 

timeLib.c,  written  by  Tom  Galluzzo) 

#include  <unistd.h>  //  Unix  standard  functions 

#include  <stdio.h>  //  Unix  standard  input-output 

#include  <math.h>  //  Mathematical  functions 

#include  Cnodemgr/nodemgr . h>  //  Node  managment  functions  for  sending  and  receiving  JAUS 
messages  (USER:  Node  Manager  must  be  installed) 


#include  "mjps.h"  //  USER:  Implement  and  rename  this  header  file.  Include  prototypes 

for  all  public  functions  contained  in  this  file. 

#include  "mcConstants . h" 

#include  "galillnterface . h"  //  Access  to  the  get JointPosition ( )  function 
#include  "logLib.h" 


#define  DATASIZE  JDATASIZE  //  Default  size  for  byte  stream  buffers  in  this  file 

//  Private  function  prototypes 
void  *mjpsStateThread (void  *)  ; 
void  *mjpsNodemgrThread (void  *) ; 

//  Query  messages  requested  by  this  component 
void  mjpsQueryPmComponentStatus (void) ; 


//  Accessor  to  outgoing  information 
jointPosition_t  *  mjpsReport JointPosition (void) ; 

unsigned  char  mjpslnstld,  mjpsNodeld,  mjpsSubsId; 
Subsystem  IDs  for  this  component 
componentAuthority_t  mjpsAuthority  =  0; 

Authority  for  this  component 
int  mjpsState  =  SHUT D0WN_S TATE ; 

State 

double  mjpsThreadHz ; 

Stores  the  calculated  update  rate  for  main  state  thread 
unsigned  char  mjpsCtrlCmptlnstld,  mjpsCtrlCmptCompId; 
component  currently  in  control 

unsigned  char  mjpsCtrlCmptNodeld,  mjpsCtrlCmptSubsId; 
component  currently  in  control 


//  JAUS  Instance,  Node  and 
//  JAUS 


//  JAUS 
// 

//  Stores  identity  of  the 
//  Stores  identity  of  the 
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unsigned  char  mjpsCtrlCmptAuthority;  //  Stores  the 

authority  level  of  the  component  currently  in  control 

int  mjpsUnderControl  =  FALSE;  // 

FALSE  -  not  under  control,  TRUE  is  under  control 
int  mjpsRun  =  FALSE; 

int  mjpsStateThreadRunning  =  FALSE; 
pthread_t  mjpsStateThreadld; 
node  manager  thread  identifier 
int  mjpsNodemgrThreadRunning  =  FALSE; 
pthread_t  mjpsNodemgrThreadld; 
pthread  node  manager  thread  identifier 

//  Structure  storing  incoming  request  under  case  QUERY_JOINT_POSITION 
static  jointPosition_t  reportedJointPosition; 

jmh_t  mjpsScHeader ; 

int  mjpsScActive  =  FALSE; 

jms_t  mjpsJms;  //  An  accessor  to  the  Node  Manager  JAUS  Message  Service  (Jms)  for  this 
component 

int  mjpsPmState  =  -1; 
unsigned  char  mjpsPmNodeld  =  0; 
int  mjpsPmControl  =  FALSE; 

static  double  curr_theta [NUM_JOINTS] ; 

//  Function:  mjpsStartup 

//  Access:  Public 

//  Description:  This  function  allows  the  abstracted  component  functionality  contained  in 
this  file  to  be  started  from  an  external  source. 

//  It  must  be  called  first  before  the  component  state  machine 

and  node  manager  interaction  will  begin 

//  Each  call  to  "mjpsStartup"  should  be  followed  by  one  call 

to  the  "cmptShutdown"  function 
int  mjpsStartup (void) 

{ 

pthread_attr_t  attr;  //  Thread  attributed  for  the  component  threads  spawned  in 
this  function 


//  pthread 

// 


if (mjpsState  ==  SHUTDOWN_STATE)  //  Execute  the  startup  routines  only  if  the 

component  is  not  running 


IDs 

SmjpsSubsId) 


//  Check  in  to  the  Node  Manager  and  obtain  Instance,  Node  and  Subsystem 

if ( jcmCheckln (MANIPULATOR_JOINT_POSITION_SENSOR,  Smjpslnstld,  SmjpsNodeld, 
<  0)  //  USER:  Insert  your  component  ID  define  on  this  line 


cError ( "mjps :  Could  not  check  in  to  nodemgr\n"); 
return  MJPS  NODE  MANAGER  CHECKIN  ERROR; 


//  Open  a  conection  to  the  Node  Manager  and  obtain  a  Jms  accessor 
if ( (mjpsJms  =  jmsOpen (MANIPULATOR_JOINT_POSITION_SENSOR,  mjpslnstld,  0) ) 
==  NULL)  //  USER:  Insert  your  component  ID  define  on  this  line 
{ 

cError ( "mjps :  Could  not  open  connection  to  nodemgr\n"); 
jcmCheckOut (MANIPULATOR_JOINT_POSITION_SENSOR,  mjpslnstld);  // 
USER:  Insert  your  component  ID  define  on  this  line 
return  MJPS_JMS_OPEN_ERROR; 

} 


mjps Jms->timeout . tv_usec  =  100000;  //  Timeout  for  non-blocking  jmsRecv, 
specified  in  micro  seconds 

pthread_attr_init (&attr) ; 

pthread_attr_setdetachstate ( &attr ,  PTHREAD_CREATE_DETACHED) ; 

mjpsState  =  INITIALIZE_STATE;  //  Set  the  state  of  the  JAUS  state  machine 

to  INITIALIZE 
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mjpsRun  =  TRUE; 

if (pthread_create ( &mjpsStateThreadId,  &attr,  mjpsStateThread,  NULL)  !=  0) 

{ 

cError ( "mjps :  Could  not  create  mjpsStateThread\n" ) ; 
mjpsShutdown ( ) ; 

return  MJPS_STATE_THREAD_CREATE_ERROR; 

} 

if (pthread_create ( &mjpsNodemgrThreadId,  &attr,  mjpsNodemgrThread,  NULL)  != 


cError ( "mjps :  Could  not  create  mjpsNodemgrThread\n" ) ; 
mjpsShutdown ( ) ; 

return  MJPS_NODEMGR_THREAD_CREATE_ERROR; 

} 

pthread_attr_destroy ( Sattr ) ; 

} 

else 

{ 

cError ( "mjps :  Attempted  startup  while  not  shutdown\n" ) ; 
return  MJPS_STARTUP_BEFORE_SHUTDOWN_ERROR; 

} 

return  0; 

} 


//  Function:  mjpsShutdown 
//  Access:  Public 

//  Description:  This  function  allows  the  abstracted  component  functionality 

contained  in  this  file  to  be  stoped  from  an  external  source. 

//  If  the  component  is  in  the  running  state,  this  function 

will  terminate  all  threads  running  in  this  file 

//  This  function  will  also  close  the  Jms  connection  to  the 

Node  Manager  and  check  out  the  component  from  the  Node  Manager 
int  mjpsShutdown (void) 

{ 

double  timeOutSec; 


if (mjpsState  !=  SHUTDOWN_STATE)  //  Execute  the  startup  routines  only  if  the 

component  is  not  running 

{ 

mjpsRun  =  FALSE; 

timeOutSec  =  getTimeSeconds ( )  +  MJPS_THREAD_TIMEOUT_SEC; 
while (mjpsStateThreadRunning) 

t 

usleep (100000) ; 

if (getTimeSeconds ( )  >=  timeOutSec) 


pthread_cancel (mjpsStateThreadld)  ; 
mjpsStateThreadRunning  =  FALSE; 

cError ( "mjps :  mjpsStateThread  Shutdown  Improperly\n" ) ; 
break; 


} 

timeOutSec  =  getTimeSeconds ( )  +  MJPSJTHREAD_TIMEOUT_SEC; 
while (mjpsNodemgrThreadRunning) 

{ 

usleep (100000) ; 

if (getTimeSeconds ( )  >=  timeOutSec) 

{ 

pthread_cancel (mjpsNodemgrThreadld) ; 
mjpsNodemgrThreadRunning  =  FALSE; 

cError ( "mjps :  mjpsNodemgrThread  Shutdown  Improperly\n" ) ; 
break; 
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//  Close  Node  Manager  Connection  and  check  out 
jmsClose (mjpsJms) ; 

j  cmCheckOut (MANIPULATOR_JOINT_POSITION_SENSOR,  mjpslnstld) ;  //  USER: 
Insert  your  component  ID  define  on  this  line 

mjps State  =  SHUT DOWN_S TATE ; 

} 

return  0; 

} 

//  The  series  of  functions  below  allow  public  access  to  essential  component  information 

//  Access:  Public  (All) 

int  mjpsGetState  (void) {  return  mjpsState;  } 

unsigned  char  mjpsGetlnstanceld (void) {  return  mjpslnstld;  } 

unsigned  char  mjpsGetCompId (void) {  return  (unsigned 

char) MANIPULATOR_JOINT_POSITION_SENSOR;  } 

unsigned  char  mjpsGetNodeld (void) {  return  mjpsNodeld;  } 

unsigned  char  mjpsGetSubsystemld (void) {  return  mjpsSubsId;  } 

double  mjpsGetUpdateRate (void) {  return  mjpsThreadHz ;  } 

int  mjpsGetPmState (void) {  return  mjpsPmState;  } 

//  USER:  Insert  any  additional  public  variable  accessors  here 

jointPosition_t  *  mjpsReport JointPosition (void)  {  return  SreportedJointPosition;  } 

//  Function:  mjpsThread 
//  Access:  Private 

//  Description:  All  core  component  functionality  is  contained  in  this  thread. 

//  All  of  the  JAUS  component  state  machine  code  can  be  found 

here . 

void  *mjpsStateThread (void  *threadData) 

{ 

jmh_t  txMsgHeader; 
double  time,  prevTime; 
unsigned  char  data [DATASIZE] ; 
int  i; 

mjpsStateThreadRunning  =  TRUE;  // 

//  Setup  transmit  header 

txMsgHeader . isExpMsg  =0;  //  not  experimental  msg  -  USER:  change  to  1 

if  experimental 

txMsgHeader . isSvcMsg  =0;  //  not  a  service  connection 

txMsgHeader . acknCtrl  =0;  //no  ack/nak 

txMsgHeader .priority  =  6;  //  default  priority 

txMsgHeader . reserved  =  0; 

txMsgHeader . version  =1;  //  JAUS  3.0 

txMsgHeader . srclnstld  =  mjpslnstld; 

txMsgHeader . srcCompId  =  MANIPULATOR_JOINT_POSITION_SENSOR; 

txMsgHeader . srcNodeld  =  mjpsNodeld; 

txMsgHeader . srcSubsId  =  mjpsSubsId; 

txMsgHeader . dataCtrl  =  0; 

txMsgHeader . seqNumber  =  0; 

time  =  getTimeSeconds ( ) ; 

mjpsState  =  INITIALIZE_STATE;  //  Set  JAUS  state  to  INITIALIZE 

while (mjpsRun)  //  Execute  state  machine  code  while  not  in  the  SHUTDOWN  state 

{ 

prevTime  =  time; 

time  =  getTimeSeconds () ; 

mjpsThreadHz  =  1 . 0/ ( time-prevTime) ;  //  Compute  the  update  rate  of  this 

thread 

switch (mjpsState)  //  Switch  component  behavior  based  on  which  state  the 

machine  is  in 

{ 

case  INITIALIZE_STATE: 

//Check  the  status  of  PRIMITIVE  MANIPULATOR 
mjpsQueryPmComponentStatus () ; 
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emergency  state"); 


txMsgHeader .dataBytes)  ; 


if (mjpsPmState  ==  EMERGENCY_STATE) 

{ 

cError ( "mjps :  PM  in  emergency  state,  switching  to 
mjpsState  =  EMERGENCY_STATE; 

} 

if (mjpsPmState  ==  STANDBY_STATE) 

{ 

txMsgHeader . cmdCode  =  RESUME; 
txMsgHeader . dstlnstld  =  1; 

txMsgHeader .dstCompId  =  PRIMITIVE_MANIPULATOR; 
txMsgHeader . dstNodeld  =  mjpsNodeld; 
txMsgHeader . dstSubsId  =  mjpsSubsId  ; 
txMsgHeader . dataBytes  =  0; 
jmsSend (mjpsJms,  & txMsgHeader ,  data. 


if (mjpsPmState  ==  READ Y_S  TATE ) 

mjpsState  =  READY^S  TATE ; 

break; 

case  STANDBY_STATE: 
break; 


case  READY_STATE: 

//Check  the  status  of  PRIMITIVE  MANIPULATOR 
mjpsQueryPmComponentStatus () ; 

if (mjpsPmState  ==  EMERGENCY_STATE) 


emergency  state") ; 


get JointPosition (i)  ; 


cError ( "mjps :  PM  in  emergency  state,  switching  to 
mjpsState  =  EMERGENCY_STATE; 

} 

//  Set  up  the  structure  returning  current  sensor  data 
reportedJointPosition . numjoints  =  NUM_JOINTS; 
for  ( i=0 ;  i<NUM_JOINTS ;  i++) 

{ 

reportedJointPosition ,jointPosition[i]  = 


} 

break; 


case  EMERGENCY_STATE; 
break; 

case  FAILURE_STATE; 
break; 

case  SHUTDOWN_STATE: 
break; 

default : 

mjpsState  =  FAILURE_STATE;  //  The  default  case  is 
undefined,  therefore  go  into  Failure  State 

break; 

} 

usleep (1000) ;  //  Sleep  for  one  millisecond 


//  Sleep  for  50  milliseconds  and  then  exit 
usleep (50000) ; 

mjpsStateThreadRunning  =  FALSE; 
pthread_exit (NULL) ; 

} 


//  Function:  mjpsNodemgrThread 
//  Access:  Private 
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//  Description:  This  thread  is  responsible  for  handling  incoming  JAUS  messages  from 

the  Node  Manager  JAUS  message  service  (Jms) 

//  incoming  messages  are  processed  according  to  message  type, 

void  *mjpsNodemgrThread (void  *threadData) 

{ 

jmh_t  rxMsgHeader,  txMsgHeader;  //  Declare  JAUS  header  data  structures,  where  the 
jmh_t  data  structure  is  defined  in  jaus.h 
unsigned  char  data [DATASIZE] ; 

createServiceConnection_t  createServiceConnection; 
conf irmServiceConnection_t  conf irmServiceConnection; 
activateServiceConnection_t  activateServiceConnection; 
suspendServiceConnection_t  suspendServiceConnection; 
terminateServiceConnection_t  terminateServiceConnection; 
requestComponentControl_t  requestComponentControl ; 
conf irmComponentControl_t  conf irmComponentControl; 
component Author ity_t  componentAuthority; 
componentStatus_t  componentStatus ; 
int  recvCount; 


mjpsNodemgrThreadRunning  =  TRUE;  // 

//  Setup  transmit  header 
txMsgHeader . isExpMsg  =  0; 
if  experimental 

txMsgHeader . isSvcMsg  =  0; 
txMsgHeader . acknCtrl  =  0; 
txMsgHeader . priority  =  6; 
txMsgHeader . reserved  =  0; 
txMsgHeader . version  =  1; 
txMsgHeader . srclnstld  =  mjpslnstld; 
txMsgHeader . srcCompId  =  MANIPULATOR 
txMsgHeader . srcNodeld  =  mjpsNodeld; 
txMsgHeader . srcSubsId  =  mjpsSubsId; 
txMsgHeader . dataCtrl  =  0; 
txMsgHeader . seqNumber  =  0; 


//  not  experimental  msg  -  USER:  change  to  1 

//  not  a  service  connection 
//  no  ack/nak 
//  default  priority 

//  JAUS  3.0 

JOINT  POSITION  SENSOR; 


while (mjpsRun)  //  Execute  incoming  JAUS  message  processing  while  not  in  the 
SHUTDOWN  state 


rxMsgHeader, 


recvCount) ; 


//  Check  for  a  new  message  in  the  Node  Manager  JMS  queue 
//  If  a  message  is  present  then  store  the  header  information  in 
and  store  the  message  content  information  in  the  data  buffer 
recvCount  =  jmsRecv (mjps Jms,  SrxMsgHeader ,  data,  DATASIZE); 
if (recvCount  <  0) 

{ 

cError ( "mjps :  Node  manager  jmsRecv  returned  error:  %d\n", 
break; 


if (recvCount  ==  0)  continue; 

//  This  block  of  code  is  intended  to  reject  commands  from  non-controlling 

components 

//  However,  the  one  exception  allowed  is  a  REQUEST_COMPONENT_CONTROL 
if (mjpsUnderControl  &&  rxMsgHeader . cmdCode  <  0x2000)  //  If  not  currently 
under  control  or  it's  not  a  command,  then  go  to  the  switch  statement 


//  If  the  source  component  isn't  the  one  in  control,  and  it's  not  a 
request  to  gain  control,  then  exit  this  iteration  of  the  while  loop 

if (  !(  mjpsCtrlCmptInstId==rxMsgHeader . srclnstld  && 

mjpsCtrlCmptCompId==rxMsgHeader . srcCompId  && 
mjpsCtrlCmptNodeId==rxMsgHeader . srcNodeld  && 
mjpsCtrlCmptSubsId==rxMsgHeader . srcSubsId) && 

!  (  rxMsgHeader . cmdCode==REQUEST_COMPONENT_CONTROL) 

) continue;  //  to  next  iteration 

} 


switch (rxMsgHeader . cmdCode)  //  Switch  the  processing  algorithm  according 
to  the  JAUS  message  type 
{ 
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code 

UNPACK) ; 


//  Set  the  component  authority  according  to  the  incoming  authority 

case  SET_COMPONENT_AUTHORITY : 

convertComponentAuthority (data,  SmjpsAuthority,  DATASIZE, 

//  Unpack  and  store  the  incoming  authority  code 
break; 

case  SHUTDOWN: 

mjpsState  =  S HUT DOWN_S TATE ; 
break; 

case  STANDBY: 

if (mjpsState  ==  READY_STATE) 

mjpsState  =  S TAN DBY_S TATE ; 

break; 

case  RESUME: 

if (mjpsState  ==  STANDBY_STATE ) 

mjpsState  =  READY_STATE; 

break; 
case  RESET: 

mjpsState  =  INITIALIZE_STATE; 
break; 

case  SET_EMERGENCY : 

mjpsState  =  EMERGENCY_STATE ; 
break; 

case  CLEAR_EMERGENCY : 

mjpsState  =  STANDBY_STATE; 
break; 


case  CREATE_SERVICE_CONNECTION: 

convertCreateServiceConnection (data, 
ScreateServiceConnection,  DATASIZE,  UNPACK) ; 

if (createServiceConnection . cmdCode  == 


RE  PORT_ JO I NT_POS I T I ON ) 


mjpsScHeader  =  txMsgHeader; 
mjpsScHeader . isSvcMsg  =  1; 

mjpsScHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
mjpsScHeader . dstCompId  =  rxMsgHeader . srcCompId; 
mjpsScHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
mjpsScHeader . dstSubsId  =  rxMsgHeader.srcSubsId; 
mjpsScHeader . cmdCode  =  REPORT_JOINT_POSITION; 
mjpsScHeader . seqNumber  =  1; 


conf irmServiceConnection . cmdCode  = 


RE  PORT_ JO I NT_POS I T I ON ; 

conf irmServiceConnection . instanceld  =  1; 
conf irmServiceConnection . conf irmedRate  = 

createServiceConnection . requestedRate;  //  BUG:  The  actual  rate  may  not  be  the  same  as  the 
request 


conf irmServiceConnection . responseCode  = 


CONNECTION  SUCCESSFUL; 


convertConf irmServiceConnection (data, 
txMsgHeader . dataBytes) ; 


txMsgHeader . cmdCode  =  CONFIRM_SERVICE_CONNECTION; 
txMsgHeader . dstSubsId  =  rxMsgHeader.srcSubsId; 
txMsgHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
txMsgHeader . dstCompId  =  rxMsgHeader . srcCompId; 
txMsgHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
txMsgHeader . dataBytes  = 

&conf irmServiceConnection,  DATASIZE,  PACK) ; 
jmsSend (mjps Jms,  StxMsgHeader ,  data. 


else 

{ 


mjpsScActive  =  TRUE; 


137 


conf irmServiceConnection . cmdCode  = 

createServiceConnection . cmdCode; 

conf irmServiceConnection . instanceld  =  1; 
conf irmServiceConnection . conf irmedRate  = 


createServiceConnection . requestedRate; 

conf irmServiceConnection . responseCode  = 


CONNECTION  REFUSED; 


convertConf irmServiceConnection (data, 
txMsgHeader . dataBytes) ; 


txMsgHeader. cmdCode  =  CONFIRM_SERVICE_CONNECTION; 
txMsgHeader . dstSubsId  =  rxMsgHeader . srcSubsId; 
txMsgHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
txMsgHeader . dstCompId  =  rxMsgHeader . srcCompId; 
txMsgHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
txMsgHeader . dataBytes  = 

&conf irmServiceConnection,  DATASIZE,  PACK) ; 
jmsSend (mjps Jms,  StxMsgHeader ,  data. 


break; 


case  CONFIRM  SERVICE  CONNECTION: 


&conf irmServiceConnection, 
connection  message 


convertConf irmServiceConnection (data, 

DATASIZE,  UNPACK) ; 

//  USER:  Insert  code  here  to  handle  the  confirm  service 


break; 


case 

SactivateServiceConnection, 
connection  message 


ACT I VATE_SERVI CE_CONNECT I ON : 

convertActivateServiceConnection (data, 

DATASIZE,  UNPACK) ; 

//  USER:  Insert  code  here  to  handle  the  activate  service 
break; 


case 

SsuspendServiceConnection, 
connection  message 


SUSPEND_SERVICE_CONNECTION : 

convert SuspendServiceConnect ion (data, 

DATASIZE,  UNPACK) ; 

//  USER:  Insert  code  here  to  handle  the  suspend  service 


break; 


case  TERMINATE  SERVICE  CONNECTION: 


SterminateServiceConnection, 
RE  PORT_ JO I NT_POS I T I ON ) 


convertTerminateServiceConnection (data, 
DATASIZE,  UNPACK) ; 

if  ( terminateServiceConnection . cmdCode  == 
mjpsScActive  =  FALSE; 

break; 


case  REQUEST_COMPONENT_CONTROL: 

convertRequestComponentControl (data, 
SrequestComponentControl,  DATASIZE,  UNPACK) ; 


if (mjpsUnderControl) 


//  Test  for  higher  authority 


if (requestComponentControl  >  mjpsCtrlCmptAuthority) 


//  Terminate  control  of  current  component 
txMsgHeader . cmdCode  = 

RE JECT_COMPONENT_CONTROL; 

txMsgHeader . dstSubsId  =  mjpsCtrlCmptSubsId; 
txMsgHeader . dstNodeld  =  mjpsCtrlCmptNodeld; 
txMsgHeader . dstCompId  =  mjpsCtrlCmptCompId; 
txMsgHeader . dstlnstld  =  mjpsCtrlCmptlnstld; 
txMsgHeader . dataBytes  =  0; 
jmsSend (mjps Jms,  StxMsgHeader ,  data, 

txMsgHeader . dataBytes)  ; 


//  Accept  control  of  new  component 
txMsgHeader . cmdCode  = 

CONFIRM  COMPONENT  CONTROL; 
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rxMsgHeader . srcSubsId; 
rxMsgHeader . srcNodeld; 
rxMsgHeader . srcCompId; 
rxMsgHeader . srclnstld; 

convertConf irmComponentControl (data,  &conf i 
txMsgHeader . dataBytes)  ; 

requestComponentControl ; 


txMsgHeader . dstSubsId  = 

txMsgHeader . dstNodeld  = 

txMsgHeader . dstCompId  = 

txMsgHeader . dstlnstld  = 

conf irmComponentControl . asByte  =  0; 
txMsgHeader . dataBytes  = 
rmComponentControl,  DATASIZE,  PACK) ; 
jmsSend (mjps Jms,  StxMsgHeader ,  data, 

mjpsCtrlCmptlnstld  =  rxMsgHeader . srclnstld; 
mjpsCtrlCmptCompId  =  rxMsgHeader . srcCompId; 
mjpsCtrlCmptNodeld  =  rxMsgHeader . srcNodeld; 
mjpsCtrlCmptSubsId  =  rxMsgHeader . srcSubsId; 
mjpsCtrlCmptAuthority  = 


else 


if  (  rxMsgHeader . srcSubsId  !  = 

mjpsCtrlCmptSubsId  | |  rxMsgHeader . srcNodeld  !=  mjpsCtrlCmptNodeld  | | 

rxMsgHeader . srcCompId  != 

mjpsCtrlCmptCompId  | |  rxMsgHeader . srclnstld  !=  mjpsCtrlCmptlnstld  ) 


RE JECT_COMPONENT_CONTROL ; 
rxMsgHeader . srcSubsId; 
rxMsgHeader . srcNodeld; 
rxMsgHeader . srcCompId; 
rxMsgHeader . srclnstld; 

txMsgHeader . dataBytes)  ; 


txMsgHeader . cmdCode  = 

txMsgHeader . dstSubsId  = 

txMsgHeader . dstNodeld  = 

txMsgHeader . dstCompId  = 

txMsgHeader . dstlnstld  = 

txMsgHeader . dataBytes  =  0; 
jmsSend (mjps Jms,  StxMsgHeader ,  data. 


control 


else  //  Not  currently  under  component  control,  so  give 


convertConf irmComponentControl (data, 
txMsgHeader . dataBytes) ; 


txMsgHeader . cmdCode  =  CONFIRM_COMPONENT_CONTROL; 

txMsgHeader . dstSubsId  =  rxMsgHeader . srcSubsId; 
txMsgHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
txMsgHeader . dstCompId  =  rxMsgHeader . srcCompId; 
txMsgHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
conf irmComponentControl . asByte  =  0; 
txMsgHeader . dataBytes  = 

&conf irmComponentControl,  DATASIZE,  PACK) ; 
jmsSend (mjps Jms,  StxMsgHeader ,  data, 

mjpsCtrlCmptlnstld  =  rxMsgHeader . srclnstld; 
mjpsCtrlCmptCompId  =  rxMsgHeader . srcCompId; 
mjpsCtrlCmptNodeld  =  rxMsgHeader . srcNodeld; 
mjpsCtrlCmptSubsId  =  rxMsgHeader . srcSubsId; 
mjpsCtrlCmptAuthority  =  requestComponentControl; 
mjpsUnderControl  =  TRUE; 


break; 


case  RELEASE_COMPONENT_CONTROL : 

mjpsUnderControl  =  FALSE; 
break; 


case  CONFIRM  COMPONENT  CONTROL: 
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&conf irmComponentControl, 
control  message  if  needed 


convertConf irmComponentControl (data, 

DATASIZE,  UNPACK) ; 

//  USER:  Insert  code  here  to  handle  the  confirm  component 


break; 


case 

control  message  if  needed 


RE JECT_COMPONENT_CONTROL : 

//  USER:  Insert  code  here  to  handle  the  reject  component 
break; 


case  QUERY_COMPONENT_AUTHORITY: 

txMsgHeader . cmdCode  =  REPORT_COMPONENT_AUTHORITY; 
txMsgHeader . dstSubsId  =  rxMsgHeader . srcSubsId; 
txMsgHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
txMsgHeader . dstCompId  =  rxMsgHeader . srcCompId; 
txMsgHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
componentAuthority  =  mjpsAuthority; 

txMsgHeader . dataBytes  =  convertComponentAuthority (data, 
ScomponentAuthority,  DATASIZE,  PACK) ; 

jmsSend (mjps Jms,  StxMsgHeader ,  data, 

txMsgHeader . dataBytes)  ; 


break; 


case  QUERY_COMPONENT_STATUS : 

txMsgHeader . cmdCode  =  REPORT_COMPONENT_STATUS; 
txMsgHeader . dstSubsId  =  rxMsgHeader . srcSubsId; 
txMsgHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
txMsgHeader . dstCompId  =  rxMsgHeader . srcCompId; 
txMsgHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
componentStatus . primaryStatus . asByte  =  mjpsState; 
txMsgHeader . dataBytes  =  convertComponentStatus (data, 
ScomponentStatus ,  DATASIZE,  PACK) ; 

jmsSend (mjps Jms,  StxMsgHeader ,  data, 

txMsgHeader . dataBytes)  ; 


break; 


DATASIZE,  UNPACK) ; 
authority  message  if 


case  REPORT_COMPONENT_AUTHORITY: 

convertComponentAuthority (data,  ScomponentAuthority, 

//  USER:  Insert  code  here  to  handle  the  report  component 

needed 

break; 


UNPACK) ; 


case  RE PORT_COMPONENT_S TATU S : 

convertComponentStatus (data. 


if (rxMsgHeader . srcCompId  == 
mjpsPmState  = 

componentStatus .primaryStatus . asField . primaryStatusCode; 

break; 


ScomponentStatus,  DATASIZE, 
PRIMITIVE_MANIPULATOR) 


case  QUERY_JOINT_POSITION: 

txMsgHeader . cmdCode  =  REPORT_JOINT_POSITION; 
txMsgHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
txMsgHeader . dstCompId  =  rxMsgHeader . srcCompId; 
txMsgHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
txMsgHeader . dstSubsId  =  rxMsgHeader . srcSubsId; 
txMsgHeader . dataBytes  =  convert JointPosition (data, 
SreportedJointPosition,  DATASIZE,  PACK) ; 

jmsSend (mjps Jms,  StxMsgHeader ,  data, 

txMsgHeader . dataBytes)  ; 

break; 

default : 

break; 


} 

mjpsNodemgrThreadRunning  =  FALSE;  // 
pthread_exit (NULL) ; 
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/ // // // // // // // // // // // // // // // // // // // // // // // // // // // // // // // // // // // // // // // // // / 
//  Function  used  to  send  a  message  to  the  primitive  manipulator  and  query  for 
//  component  status.  Only  the  primitive  manipulator  component  performs  the  initial 
//  system  checks.  Thus  it  is  essential  that  primitive  manipulator  is  running  and  its 
//  status  is  ready  before  any  other  components  are  started  up. 
//////////////////////////////////////////////////////////////////////////////////// 
void  mjpsQueryPmComponentStatus (void) 

{ 

j  rnh  _t  txMsg; 

unsigned  char  data [DATASIZE] ; 
double  requestTimeOutSec  =  0.1; 
static  double  lastRequestTimeSec  =  0; 

//  Setup  Transmit  msg 

txMsg . isExpMsg  =  0; 

txMsg . priority  =  6; 

txMsg. acknCtrl  =  0; 

txMsg . isSvcMsg  =  0; 

txMsg . version  =  1; 

txMsg . reserved  =  0; 

txMsg . srclnstld  =  mjpslnstld; 

txMsg. srcCompId  =  MANIPULATOR_JOINT_POSITION_SENSOR; 

txMsg . srcNodeld  =  mjpsNodeld; 

txMsg. srcSubsId  =  mjpsSubsId; 

txMsg. dataCtrl  =  0; 

txMsg . seqNumber  =  0; 

txMsg. cmdCode  =  QUERY_COMPONENT_STATUS; 
txMsg . dstlnstld  =  1; 

txMsg. dstcompld  =  PRIMITIVE_MANIPULATOR; 
txMsg . dstNodeld  =  mjpsNodeld; 
txMsg. dstSubsId  =  mjpsSubsId  ; 
txMsg. dataBytes  =  0; 

if (  (getTimeSeconds ( )  -  lastRequestTimeSec)  >=  requestTimeOutSec) 

{ 

jmsSend (mjpsJms,  &txMsg,  data,  txMsg. dataBytes ) ; 
lastRequestTimeSec  =  getTimeSeconds () ; 


///////////////////////////////////////////////////////////////////////////////////////// 

//  File:  mjps.h 

//  Version:  0.1  Original  Creation 

//  Written  by:  Ognjen  Sosa  (ognjensosa@hotmail.com) 


// 

// 

// 

// 

// 


Template  by: 
Date : 

Description : 


Tom  Galluzzo 
09/28/2004 


(galluzzt@ufl. edu) 


This  file  contains  the  skeleton  C  header  file  code  for  implementing 
the  mjps.c  file 


///////////////////////////////////////////////////////////////////////////////////////// 


#ifndef  MJPS_H 
#define  MJPS_H 

#include  "jaus.h" 

#ifndef  FALSE 
#def ine  FALSE  0 
#endif 

#ifndef  TRUE 
#define  TRUE  1 
#endif 


#def ine  MJPS_NODE_MANAGER_CHECKIN_ERROR  -1 

#def ine  MJPS_JMS_OPEN_ERROR  -2 

#def ine  MJPS_STARTUP_BEFORE_SHUTDOWN_ERROR  -3 
#def ine  MJPS_S TATE_THREAD_CREATE_ERROR 
#def ine  MJPS  NODEMGR  THREAD  CREATE  ERROR 


-5 


-4 
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#def ine  MJPS_THREADJTIMEOUT_SEC  1.0 

//  Public 

int  mjpsStartup (void) ; 

int  mjpsShutdown (void) ; 

int  mjpsGetState (void)  ; 

unsigned  char  mjpsGetlnstanceld (void) ; 

unsigned  char  mjpsGetCompId (void) ; 

unsigned  char  mjpsGetNodeld (void) ; 

unsigned  char  mjpsGetSubsystemld (void) ; 

double  mjpsGetUpdateRate (void)  ; 

int  mjpsGetPmState (void)  ; 

//  Query  messages 

void  mjpsQueryPmComponentStatus (void) ; 

//  Private  accessors 

jointPosition_t  *  mjpsReport JointPosition (void) ; 

#endif  //  MJPS_H 

B.4  The  Meepd.c  File  and  the  Corresponding  Header  Meepd.h 


//  File:  meepd.c 

//  Version:  0.1  Original  Creation 

//  Written  by:  Ognjen  Sosa  (ognjensosa@hotmail.com) 


Template  by: 
Date : 


// 

// 

// 

//  Description: 
// 

// 

// 


Tom  Galluzzo  (galluzzt@ufl.edu) 

09/28/2004 

This  file  contains  the  C  code  for  implementation  of  a  Manipulator 
Joint  Move  Driver  JAUS  component  in  a  Linux  environment.  This  code 
is  designed  to  work  with  the  node  manager  and  JAUS  library  software 
written  by  Jeff  Witt 

///////////////////////////////////////////////////////////////////////////////////////// 


//  JAUS  message  set  (USER:  JAUS  libraries 


//  Multi-threading  functions  (standard  to  unix) 
//  Precise  timing  routines  (USER:  must  link 


#include  <jaus.h> 
must  be  installed  first) 

#include  <pthread.h> 

#include  <timeLib.h> 
timeLib.c,  written  by  Tom  Galluzzo) 

#include  <unistd.h>  //  Unix  standard  functions 

#include  <stdio.h>  //  Unix  standard  input-output 

#include  <math.h>  //  Mathematical  functions 

#include  <nodemgr/nodemgr ,h>  //  Node  managment  functions  for  sending  and  receiving  JAUS 

messages  (USER:  Node  Manager  must  be  installed) 


#include  "meepd.h"  //  USER:  Implement  and  rename  this  header  file.  Include  prototypes 
for  all  public  functions  contained  in  this  file. 

#include  "mcConstants .h" 

#include  "galillnterface.h"  //  Access  to  setMaxSpeed ( )  function 
#include  "cpplnterface.h" 

#include  "logLib.h" 

#define  DATASIZE  JDATASIZE  //  Default  size  for  byte  stream  buffers  in  this  file 

//  Private  function  prototypes 
void  *meepdStateThread (void  *); 
void  *meepdNodemgrThread ( void  *); 

//  Query  messages  requested  by  this  component 

void  meepdQueryPmComponentStatus (void)  ; 

void  meepdQueryPmJointEf f ort (void) ; 

void  meepdQueryMjps JointPosition (void)  ; 

void  meepdQueryPmManipulatorSpecif ications (void) ; 

//  Set  messages  that  this  component  commands 
void  meepdSetPmJointEf f ort (void) ; 
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//  Accessors  to  incoming  information 
toolPoint_t  *  meepdSetToolPoint (void) ; 
endEf fectorPose_t  *  meepdSetEndEf fectorPose (void) ; 
jointEf fort_t  *  meepdReport JointEf fort (void) ; 

manipulatorSpecif ications_t  *  meepdReportManipulatorSpecif ications (void) ; 
jointPosition_t  *  meepdReport JointPosition (void) ; 

//  Accessors  to  outgoing  information 
toolPoint_t  *  meepdReportToolPoint (void) ; 
jointEf fort_t  *  meepdSet JointEf fort (void) ; 

//  Accessots  to  other  information 
double  *  meepdGetCurrentOrientation (void) ; 

unsigned  char  meepdlnstld,  meepdNodeld,  meepdSubsId; 

Subsystem  IDs  for  this  component 
componentAuthority_t  meepdAuthority  =  0; 

Authority  for  this  component 
int  meepdState  =  S HUT DOWN_S TATE ; 

State 

double  meepdThreadHz ; 

Stores  the  calculated  update  rate  for  main  state  thread 
unsigned  char  meepdCtrlCmptlnstld,  meepdCtrlCmptCompId; 
component  currently  in  control 

unsigned  char  meepdCtrlCmptNodeld,  meepdCtrlCmptSubsId; 
component  currently  in  control 
unsigned  char  meepdCtrlCmptAuthority; 

authority  level  of  the  component  currently  in  control 
int  meepdUnderControl  =  FALSE; 

FALSE  -  not  under  control,  TRUE  is  under  control 
int  meepdRun  =  FALSE; 

int  meepdStateThreadRunning  =  FALSE; 
pthread_t  meepdStateThreadld; 
node  manager  thread  identifier 
int  meepdNodemgrThreadRunning  =  FALSE; 
pthread_t  meepdNodemgrThreadld; 
pthread  node  manager  thread  identifier 

//  Structure  storing  incoming  request  under  case  SET  TOOL  POINT 
static  toolPoint_t  setToolPoint_6; 

//  Structure  storing  incoming  request  under  case  SET  END  EFFECTOR  POSE 
static  endEf fectorPose_t  setEndEf fectorPose; 

//  Structure  storing  incoming  request  under  case  QUERY_TOOL_POINT 
static  toolPoint_t  reportedToolPoint; 

//  Structures  storing  queried  information 
static  jointEf fort_t  reportedJointEf fort ; 

static  manipulatorSpecif i cat ions_t  reportedManipulatorSpecif ications; 
static  jointPosition_t  reportedJointPosition; 

//  Structure  storing  outgoing  request  to  the  PRIMITIVE  MANIPULATOR 
static  jointEf fort_t  set JointEf fort ; 

static  double  max_speed [NUM_JOINTS] ,  max_acceleration [NUM_JOINTS] , 

max_deceleration  [NUM__JOINTS]  ; 

static  double  cmd_pos it ion [NUM_ JOINTS ] ; 

static  int  num_soln; 

static  double  orients [3],  orientA[3]; 

static  double  des_position [3] ,  des_orientS [3] ,  des_orientA [3] ,  curr_orientation [ 4 ] ; 
jmh_t  meepdScHeader ; 

jms_t  meepdJms;  //  An  accessor  to  the  Node  Manager  JAUS  Message  Service  (Jms)  for 

this  component 

int  meepdScActive  =  FALSE; 

int  meepdPmState  =  -1; 
unsigned  char  meepdPmNodeld  =  0; 
int  meepdPmControl  =  FALSE; 


//  pthread 

// 


//  JAUS  Instance,  Node  and 
//  JAUS 

//  JAUS 
// 

//  Stores  identity  of  the 
//  Stores  identity  of  the 

//  Stores  the 
// 
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//  Function:  meepdStartup 

//  Access:  Public 

//  Description:  This  function  allows  the  abstracted  component  functionality  contained  in 
this  file  to  be  started  from  an  external  source. 

//  It  must  be  called  first  before  the  component  state  machine 

and  node  manager  interaction  will  begin 

//  Each  call  to  "meepdStartup"  should  be  followed  by  one  call 

to  the  "cmptShutdown"  function 
int  meepdStartup (void) 

{ 

pthread_attr_t  attr;  //  Thread  attributed  for  the  component  threads  spawned  in 
this  function 


if (meepdState  ==  SHUTDOWN_STATE)  //  Execute  the  startup  routines  only  if  the 

component  is  not  running 

{ 


//  Check  in  to  the  Node  Manager  and  obtain  Instance,  Node  and  Subsystem 


IDs 


if ( jcmCheckln (MANIPULATOR_END_EFFECTOR_POSE_DRIVER,  Smeepdlnstld, 
&meepdNode!d,  SmeepdSubsId)  <  0)  //  USER:  Insert  your  component  ID  define  on  this  line 


cError ( "meepd :  Could  not  check  in  to  nodemgr\n"); 
return  MEEPD  NODE  MANAGER  CHECKIN  ERROR; 


//  Open  a  conection  to  the  Node  Manager  and  obtain  a  Jms  accessor 
if ( (meepdJms  =  jmsOpen (MANIPULATOR_END_EFFECTOR_POSE_DRIVER,  meepdlnstld, 
0) )  ==  NULL)  //  USER:  Insert  your  component  ID  define  on  this  line 
{ 

cError ( "meepd :  Could  not  open  connection  to  nodemgr\n"); 
jcmCheckOut (MANIPULATOR_END_EFFECTOR_POSE_DRIVER,  meepdlnstld) ;  // 
USER:  Insert  your  component  ID  define  on  this  line 
return  MEEPD_JMS_OPEN_ERROR; 

} 


meepdJms->timeout . tv_usec  =  100000;  //  Timeout  for  non-blocking  jmsRecv, 
specified  in  micro  seconds 

pthread_attr_init (&attr)  ; 

pthread_attr_setdetachstate ( &attr ,  PTHREAD_CREATE_DETACHED) ; 

meepdState  =  INITIALIZE_STATE;  //  Set  the  state  of  the  JAUS  state  machine 

to  INITIALIZE 

meepdRun  =  TRUE; 

if (pthread_create ( SmeepdStateThreadld,  &attr,  meepdStateThread,  NULL)  != 


cError ( "meepd:  Could  not  create  meepdStateThread\n" ) ; 
meepdShutdown ( ) ; 

return  MEEPD  STATE  THREAD  CREATE  ERROR; 


!=  0) 


if (pthread_create ( SmeepdNodemgrThreadld,  &attr,  meepdNodemgrThread,  NULL) 


cError ( "meepd :  Could  not  create  meepdNodemgrThread\n" ) ; 
meepdShutdown ( ) ; 

return  MEEPD  NODEMGR  THREAD  CREATE  ERROR; 


pthread_attr_destroy ( &attr) ; 

} 

else 


cError ( "meepd :  Attempted  startup  while  not  shutdown\n" ) ; 
return  MEEPD  STARTUP  BEFORE  SHUTDOWN  ERROR; 


return  0; 
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} 

//  Function:  meepdShutdown 
//  Access:  Public 

//  Description:  This  function  allows  the  abstracted  component  functionality 

contained  in  this  file  to  be  stoped  from  an  external  source. 

//  If  the  component  is  in  the  running  state,  this  function 

will  terminate  all  threads  running  in  this  file 

//  This  function  will  also  close  the  Jms  connection  to  the 

Node  Manager  and  check  out  the  component  from  the  Node  Manager 
int  meepdShutdown (void) 

{ 

double  timeOutSec; 

if (meepdState  !=  SHUTDOWN_STATE)  //  Execute  the  startup  routines  only  if  the 

component  is  not  running 

{ 

meepdRun  =  FALSE; 

timeOutSec  =  getTimeSeconds ( )  +  MEEPD_THREAD_TIMEOUT_SEC; 
while (meepdStateThreadRunning) 

{ 

usleep (100000) ; 

if (getTimeSeconds ( )  >=  timeOutSec) 

{ 

pthread_cancel (meepdStateThreadld)  ; 
meepdStateThreadRunning  =  FALSE; 

cError ( "meepd :  meepdStateThread  Shutdown  Improperly\n" ) ; 
break; 

} 

} 


timeOutSec  =  getTimeSeconds ( )  +  MEEPD_THREAD_TIMEOUT_SEC; 
while (meepdNodemgrThreadRunning) 

{ 

usleep (100000) ; 

if (getTimeSeconds ( )  >=  timeOutSec) 

{ 

pthread_cancel (meepdNodemgrThreadld)  ; 
meepdNodemgrThreadRunning  =  FALSE; 

cError ( "meepd :  meepdNodemgrThread  Shutdown  Improperly\n" ) ; 
break; 

} 

} 


//  Close  Node  Manager  Connection  and  check  out 
jmsClose (meepdJms) ; 

jcmCheckOut (MANIPULATOR_END_EFFECTOR_POSE_DRIVER,  meepdlnstld) ;  //  USER: 
Insert  your  component  ID  define  on  this  line 

meepdState  =  SHUTDOWN_STATE; 

} 


return 


0; 


//  The  series  of  functions  below  allow  public  access  to  essential  component  information 

//  Access:  Public  (All) 

int  meepdGetState (void) {  return  meepdState;  } 

unsigned  char  meepdGetlnstanceld ( void) {  return  meepdlnstld;  } 

unsigned  char  meepdGetCompId ( void) {  return  (unsigned 

char ) MANIPULATOR_END_EFFECTOR_POSE_DRIVER;  } 

unsigned  char  meepdGetNodeld (void) {  return  meepdNodeld;  } 

unsigned  char  meepdGetSubsystemld ( void) {  return  meepdSubsId;  } 

double  meepdGetUpdateRate (void) {  return  meepdThreadHz;  } 

int  meepdGetPmState (void) {  return  meepdPmState;  } 

//  USER:  Insert  any  additional  public  variable  accessors  here 
toolPoint_t  *  meepdSetToolPoint (void)  {return  &setToolPoint_6 ;  } 

endEf f ectorPose_t  *  meepdSetEndEf fectorPose (void)  {return  SsetEndEf fectorPose;  } 
jointEf fort_t  *  meepdReport JointEf fort (void)  {  return  &reportedJointEf fort;  } 
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manipulatorSpecif ications_t  *  meepdReportPmManipulatorSpecif ications (void)  {  return 
&reportedManipulator Specifications;  } 

jointPosition_t  *  meepdReport JointPosition ( void)  {  return  &reportedJointPosition;  } 
toolPoint_t  *  meepdReportToolPoint (void)  {  return  &reportedToolPoint;  } 
jointEf fort_t  *  meepdSet JointEf fort (void)  {  return  &set JointEf fort;  } 

double  *  meepdGetCurrentOrientation (void)  {  return  curr_orientation;  } 

//  Function:  meepdThread 
//  Access:  Private 

//  Description:  All  core  component  functionality  is  contained  in  this  thread. 

//  All  of  the  JAUS  component  state  machine  code  can  be  found 

here . 

void  *meepdStateThread (void  *threadData) 


jmh_t  txMsgHeader; 
double  time,  prevTime; 
unsigned  char  data [DATASIZE] ; 
int  i; 

double  curr_position [NUM_JOINTS] ,  curr_ef f ort [NUM_JOINTS ] ,  cmd_ef f ort [NUM_JOINTS ] , 
curr_tool_point [3]  ,  tool_point_6 [ 3 ] ,  cmd_position [ 3 ] ,  cmd_orientation [ 4 ] ; 

double  spec_a [NUM_J0INTS-1] ,  spec_alpha [NUM_J0INTS-1 ] ,  spec_S [NUM_JOINTS] ; 
meepdStateThreadRunning  =  TRUE;  // 


//  Setup  transmit  header 
txMsgHeader . isExpMsg  =  0;  // 

if  experimental 

txMsgHeader . isSvcMsg  =  0;  // 

txMsgHeader . acknCtrl  =  0;  // 

txMsgHeader .priority  =  6;  // 

txMsgHeader . reserved  =  0; 

txMsgHeader . version  =1;  // 


txMsgHeader . srclnstld  =  meepdlnstld; 
txMsgHeader. srcCompId  =  MANIPULATOR_END 
txMsgHeader . srcNodeld  =  meepdNodeld; 
txMsgHeader . srcSubsId  =  meepdSubsId; 
txMsgHeader . dataCtrl  =  0; 
txMsgHeader . seqNumber  =  0; 


not  experimental  msg  -  USER:  change  to  1 

not  a  service  connection 
no  ack/nak 
default  priority 

JAUS  3 . Ou 

EFFECTOR  POSE  DRIVER; 


time  =  getTimeSeconds ( ) ; 

meepdState  =  INITIALIZE_STATE;  //  Set  JAUS  state  to  INITIALIZE 


while (meepdRun)  //  Execute  state  machine  code  while  not  in  the  SHUTDOWN  state 

{ 

prevTime  =  time; 

time  =  getTimeSeconds () ; 

meepdThreadHz  =  1 . 0/ (time-prevTime) ;  //  Compute  the  update  rate  of  this 

thread 


switch (meepdState)  //  Switch  component  behavior  based  on  which  state  the 

machine  is  in 

{ 

case  INITIALIZE_STATE: 

//Check  the  status  of  PRIMITIVE  MANIPULATOR 
meepdQueryPmComponentStatus ( ) ; 
if (meepdPmState  ==  EMERGENCY_STATE) 


emergency  state"); 


cError ( "meepd :  PM  in  emergency  state,  switching  to 
meepdState  =  EMERGENCY_STATE; 


if (meepdPmState  ==  STANDBY_STATE) 

{ 

txMsgHeader . cmdCode  =  RESUME; 
txMsgHeader . dstlnstld  =  1; 

txMsgHeader .dstCompId  =  PRIMITIVE_MANIPULATOR; 
txMsgHeader . dstNodeld  =  meepdNodeld; 
txMsgHeader . dstSubsId  =  meepdSubsId  ; 
txMsgHeader . dataBytes  =  0; 
jmsSend (meepdJms,  & txMsgHeader ,  data. 


txMsgHeader .dataBytes)  ; 
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} 

if (meepdPmState  ==  READ Y_S  TATE ) 

meepdState  =  STANDBY_STATE; 

break; 

case  STANDBY_STATE: 
break; 


values 


case  READY_STATE: 

//  Set  up  default  velocity,  acceleration,  and  deceleration 
for  (1=0;  i<NUM_JOINTS;  i++) 


Jl_RAD_TO_ENC 
city  /  1000; 

J1  RAD  TO  ENC; 


if  (1  ==  0) { 

max_speed[i]  =  5000  / 

//reportedManipulatorSpecif ications . j ointSpecif ications [i] . j ointMaximumVelo 

max  acceleration [i]  =  150000  / 


J2  RAD  TO  ENC; 


J3  RAD  TO  ENC; 


J4  RAD  TO  ENC; 


J5  RAD  TO  ENC; 


else  if  (i  ==  1)  { 

max_speed[i]  =  5000  /  J2_RAD_TO_ENC; 
max  acceleration [i]  =  150000  / 


else  if  (i  ==  2)  { 

max_speed[i]  =  5000  /  J3_RAD_TO_ENC; 
max_acceleration [i]  =  150000  / 

} 

else  if  (i  ==  3)  { 

max_speed[i]  =  1000  /  J4_RAD_TO_ENC; 
max  acceleration [i]  =  150000  / 


else  if  (i  ==  4)  { 

max_speed[i]  =  1000  /  J5_RAD_TO_ENC; 
max  acceleration [i]  =  150000  / 


J6_RAD_TO_ENC 
J6  RAD  TO  ENC 


else  { 

max_speed[i]  =  1000  / 

//reportedManipulatorSpecif ications . j ointNmaximumVelocity  /  1000; 

max_acceleration [i]  =  150000  / 

} 

max  deceleration [ i ]  =  max  acceleration [i] ; 


emergency  state") ; 


setMaxSpeed (max_speed)  ; 
setMaxAcceleration (max_acceleration) ; 
setMaxDeceleration (max_deceleration) ; 

//Check  the  status  of  PRIMITIVE  MANIPULATOR 
meepdQueryPmComponentStatus ( ) ; 
if (meepdPmState  ==  EMERGENCY_STATE) 

{ 

cError ( "meepd :  PM  in  emergency  state,  switching  to 
meepdState  =  EMERGENCY_STATE; 

} 


//  Query  for  required  information 
meepdQueryMjps JointPosition ( ) ; 
meepdQueryPmManipulatorSpecif ications () ; 
meepdQueryPmJointEf f ort  () ; 

for  ( i=0 ;  i<NUM_JOINTS ;  i++) 

{ 

curr_position [i]  = 

reported Joint Posit ion . j oint Posit ion [ i ]  ; 

curr_effort [i]  =  reportedJointEf fort . j ointEf fort [ i ] ; 
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} 

for ( i=0 ;  i<NUM_JOINTS-l ;  i++) 

{ 

spec_a[i]  = 

reportedManipulator Specifications . j  oint Specifications [ i ]  . linkLength; 

spec_alpha [i]  = 

reportedManipulator Specifications . j oint Specif i cat ions [i] . twistAngle/1000; 

if  ( i<3  | |  i==4 ) 

spec_S[i]  = 

reportedManipulatorSpecif ications . j ointSpecif ications [i] . jointOffsetOrAngle; 

else  if  (i==3) 

spec_S[i]  =  - 

(reportedManipulatorSpecif ications . j ointSpecif ications [i]  . jointOffsetOrAngle) ; 

} 

spec_S[5]  =  - 

(reportedManipulatorSpecif ications . jointNoffsetOrAngle)  ; 

//  Extract  incoming  tool  point  in  6th  coordinate 
tool_point_6 [0]  =  setToolPoint_6 .X  *  1000;  //  mm 
tool_point_6 [ 1 ]  =  setToolPoint_6 . Y  *  1000; 
tool_point_6 [2 ]  =  setToolPoint_6 . Z  *  1000; 

cDebug(9,  "Set  Tool  Point  %lf\n",  tool_point_6 [0]  ); 

cDebug(9,  "Set  Tool  Point  %if\n",  tool_point_6 [ 1 ]  ); 

cDebug(9,  "Set  Tool  Point  %lf\n",  tool_point_6 [2]  ); 

cDebug(9,  "\n"); 


orientation  of  the 
tool_point_6,  curr 
orients,  orientA) ; 


//  This  function  returns  the  current  position  and 
end-effector 

getCurrentEndEf fectorPose (spec_a,  spec_alpha,  spec_S, 


position. 


curr_tool_point. 


/* 

reportedToolPoint.X  ); 
reportedToolPoint . Y  ); 
reportedToolPoint . Z  ); 


*/ 

/* 


matrixToQuaternion (orientA,  orients,  curr_orientation) 


//  Report  Tool  Point 


reportedToolPoint . X 
reportedToolPoint . Y 
reportedToolPoint . Z 


=  curr_tool_point  [0] ; 
=  curr_tool_point [ 1 ] ; 
=  curr_tool_point [2 ] ; 


cDebug 

(9, 

"Reported 

Tool  Poisiton 

%  1  f  \  n  " , 

cDebug 

(9, 

"Reported 

Tool  Poisiton 

%  1  f  \  n  " , 

cDebug 

(9, 

"Reported 

Tool  Poisiton 

%lf \n" , 

cDebug 

(9, 

"\n" ) ; 

cDebug 

(9, 

"Reported 

Orientation 

s 

%  1  f  \  n  " , 

.  orients  I 

0 

cDebug 

(9, 

"Reported 

Orientation 

s 

%lf \n" , 

.  orients  I 

1 

cDebug 

(9, 

"Reported 

Orientation 

s 

%  1  f  \  n  " , 

.  orients | 

2 

cDebug 

(9, 

"\n" ) ; 

cDebug 

(9, 

"Reported 

Orientation 

A 

%  1  f  \  n  " , 

.  orientA | 

0 

cDebug 

(9, 

"Reported 

Orientation 

A 

%  1  f  \  n  " , 

.  orientA | 

1 

cDebug 

(9, 

"Reported 

Orientation 

A 

%  1  f  \  n  " , 

.  orientA | 

2 

cDebug 

(9, 

"\n" ) ; 

des_position [0]  =  849; 
des_position [ 1 ]  =  473; 
des_position [2 ]  =  879; 

des_orientS [0]  =  -0.674398; 
des_orientS [ 1 ]  =  -0.729044; 
des_orientS [2]  =  -0.116967; 

des_orientA[0]  =  0.732137; 
des_orientA [ 1 ]  =  -0.680800; 
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*/ 

/* 


des_orientA [2 ]  =  0.022070; 


des_position [0] 
des_position [ 1 ] 
des_position [2 ] 

des_orientS [0] 
des_orientS [ 1 ] 
des_orientS [2] 

des_orientA[0] 
des_orientA [ 1 ] 
des_orientA [2 ] 


=  908; 

=  -602; 

=  512; 

=  -0.6229841; 
=  0.68939257; 
=  -0.3696331; 

=  -0.4888207; 
=  -0.02581027 
=  0.87200234; 


matrixToQuaternion (des_orientA,  des_orientS, 

cmd_orientation) ; 

*/  //  Extract  information  about  desired  position  and 

orientation 

des_position [ 0 ]  =  setEndEffectorPose.X  *  1000;  //  mm 
des_position [ 1 ]  =  setEndEf fectorPose . Y  *  1000; 
des_position [2 ]  =  setEndEf fectorPose . Z  *  1000; 
cmd_orientation [0]  = 

setEndEf fectorPose . quaternionQcomponentD; 

cmd_orientation [ 1 ]  = 

setEndEf fectorPose . quaternionQcomponentA; 

cmd_orientation [ 2 ]  = 

setEndEf fectorPose . quaternionQcomponentB; 

cmd_orientation [3]  = 

setEndEf fectorPose . quaternionQcomponentC; 


des_orientA) ; 


cDebug (9, 
cDebug (9, 
cDebug (9, 
cDebug (9, 


Set  Tool  Point  %lf\n". 
Set  Tool  Point  %lf\n". 
Set  Tool  Point  %lf\n", 
\n")  ; 


des_position [0]  ); 
des_position [ 1 ]  ); 
des_position [2]  ); 


quaternionToMatrix (cmd_orientation,  des_orientS, 


the  desired  pose 
des_orientA,  cmd__position)  ; 


//  This  function  returns  the  joint  angles  corresponding  to 
getDesiredJointAngles (des_position,  des_orientS, 


cmd_position [0] * J1 
cmd_position [ 1 ] * J2 
cmd_position [2 ] * J3 
cmd_position [3] * J4 
cmd_position [4] *J5 
cmd_position [5] * J6 


RAD_TO_ENC) ; 

c 

RAD_TO_ENC) ; 

c 

RAD_TO_ENC) ; 

c 

RAD_TO_ENC) ; 

c 

RAD_TO_ENC) ; 

c 

RAD  TO  ENC) ; 


cDebug (9, 

"  Cmd 

Positions 

are : 

%lf\n 

cDebug (9, 

"  Cmd 

Positions 

are : 

%lf\n 

cDebug (9, 

"  Cmd 

Positions 

are : 

%lf\n 

cDebug (9, 

"  Cmd 

Positions 

are : 

%lf\n 

cDebug (9, 

"  Cmd 

Positions 

are : 

%lf\n 

cDebug (9, 

"  Cmd 

Positions 

are : 

%lf\n 

control 
cmd_effort) ; 


//  This  function  performs  closed  loop  end  effector  pose 
endEf f ectorPoseDriver (cmd_position,  curr_position. 


//  Set  Joint  Efforts 

set JointEf fort . numjoints  =  NUM_JOINTS; 
for  ( i=0 ;  i<NUM_JOINTS ;  i++) 

{ 

set JointEf fort . jointEf fort [i]  =  cmd_ef f ort [ i ] ; 

} 

meepdSetPmJointEf f ort ( )  ; 


break; 
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case  EMERGENCY_STATE : 
break; 

case  FAILURE_STATE : 
break; 

case  SHUTDOWN_STATE : 
break; 

default : 

meepdState  =  FAILURE_STATE;  //  The  default  case  is 
undefined,  therefore  go  into  Failure  State 

break; 

} 

usleep (1000) ;  //  Sleep  for  one  millisecond 

} 

//  Sleep  for  50  milliseconds  and  then  exit 
usleep (50000) ; 

meepdStateThreadRunning  =  FALSE; 
pthread_exit (NULL) ; 

} 


//  Function:  meepdNodemgrThread 
//  Access:  Private 

//  Description:  This  thread  is  responsible  for  handling  incoming  JAUS  messages  from 

the  Node  Manager  JAUS  message  service  (Jms) 

//  incoming  messages  are  processed  according  to  message  type, 

void  ^meepdNodemgrThread (void  *threadData) 


jmh_t  rxMsgHeader,  txMsgHeader;  //  Declare  JAUS  header  data  structures,  where  the 
jmh_t  data  structure  is  defined  in  jaus.h 
unsigned  char  data [DATASIZE] ; 

createServiceConnection_t  createServiceConnection; 
conf irmServiceConnection_t  conf irmServiceConnection; 
activateServiceConnection_t  activateServiceConnection; 
suspendServiceConnection_t  suspendServiceConnection; 
terminateServiceConnection_t  terminateServiceConnection; 
requestComponentControl_t  requestComponentControl ; 
conf irmComponentControl_t  conf irmComponentControl ; 
component Author ity_t  componentAuthority; 
componentStatus_t  componentStatus ; 
int  recvCount; 


meepdNodemgrThreadRunning  =  TRUE;  // 


//  Setup  transmit  header 
txMsgHeader . isExpMsg  =  0; 
if  experimental 

txMsgHeader . isSvcMsg  =  0; 
txMsgHeader . acknCtrl  =  0; 
txMsgHeader . priority  =  6; 
txMsgHeader . reserved  =  0; 
txMsgHeader . version  = 
txMsgHeader . srclnstld 
txMsgHeader . srcCompId 
txMsgHeader . srcNodeld  =  meepdNodeld; 
txMsgHeader . srcSubsId  =  meepdSubsId; 
txMsgHeader . dataCtrl  =  0; 
txMsgHeader . seqNumber  =  0; 


//  not  experimental  msg  -  USER:  change  to  1 

//  not  a  service  connection 
//  no  ack/nak 
//  default  priority 


1;  //  JAUS  3.0 

=  meepdlnstld; 

=  MANIPULATOR  END  EFFECTOR  POSE  DRIVER; 


while (meepdRun)  //  Execute  incoming  JAUS  message  processing  while  not  in  the 
SHUTDOWN  state 


//  Check  for  a  new  message  in  the  Node  Manager  JMS  queue 
//  If  a  message  is  present  then  store  the  header  information  in 
rxMsgHeader,  and  store  the  message  content  information  in  the  data  buffer 

recvCount  =  jmsRecv (meepdJms,  SrxMsgHeader ,  data,  DATASIZE); 
if (recvCount  <  0) 
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recvCount) ; 


{ 

cError ( "meepd :  Node  manager  jmsRecv  returned  error:  %d\n", 
break; 

} 

if (recvCount  ==  0)  continue; 


//  This  block  of  code  is  intended  to  reject  commands  from  non-controlling 

components 

//  However,  the  one  exception  allowed  is  a  REQUEST_COMPONENT_CONTROL 
if (meepdUnderControl  &&  rxMsgHeader . cmdCode  <  0x2000)  //  If  not  currently 
under  control  or  it's  not  a  command,  then  go  to  the  switch  statement 


//  If  the  source  component  isn't  the  one  in  control,  and  it's  not  a 
request  to  gain  control,  then  exit  this  iteration  of  the  while  loop 

if (  !(  meepdCtrlCmptInstId==rxMsgHeader . srclnstld  && 

meepdCtrlCmptCompId==rxMsgHeader . srcCompId  && 
meepdCtrlCmptNodeId==rxMsgHeader . srcNodeld  && 
meepdCtrlCmptSubsId==rxMsgHeader . srcSubsId) && 

!  (  rxMsgHeader . cmdCode==REQUEST_COMPONENT_CONTROL) 

) continue;  //  to  next  iteration 

} 


switch (rxMsgHeader . cmdCode)  //  Switch  the  processing  algorithm  according 
to  the  JAUS  message  type 


code 

UNPACK) ; 


//  Set  the  component  authority  according  to  the  incoming  authority 
case  SET_COMPONENT_AUTHORITY : 

convertComponentAuthority (data,  SmeepdAuthority,  DATASIZE, 
//  Unpack  and  store  the  incoming  authority  code 
break; 


case  SHUTDOWN: 

meepdState  =  S HUT DOWN_S TATE ; 
break; 


case  STANDBY: 

if (meepdState  ==  READY_STATE) 

meepdState  =  STANDBY_STATE; 

break; 


case  RESUME: 

if (meepdState  ==  STANDBY_STATE) 

meepdState  =  READY_STATE; 

break; 


case  RESET: 

meepdState  =  INITIALIZE_STATE; 
break; 


case  SET_EMERGENCY : 

meepdState  =  EMERGENCY_STATE; 
break; 


case  CLEAR_EMERGENCY : 

meepdState  =  STANDBY_STATE; 
break; 


case  CREATE_SERVICE_CONNECTION: 

convertCreateServiceConnection (data, 
ScreateServiceConnection,  DATASIZE,  UNPACK) ; 

if (createServiceConnection . cmdCode  == 


RE  PORT_ JO I NT_VELOC I T Y ) 


meepdScHeader  =  txMsgHeader; 
meepdScHeader . isSvcMsg  =  1; 

meepdScHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
meepdScHeader . dstCompId  =  rxMsgHeader . srcCompId; 
meepdScHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
meepdScHeader . dstSubsId  =  rxMsgHeader . srcSubsId; 
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meepdScHeader . cmdCode  =  REPORT_JOINT_VELOCITY; 
meepdScHeader . seqNumber  =  1; 


conf irmServiceConnection . cmdCode  = 


REPORT_JOINT_VELOCITY; 

conf irmServiceConnection . instanceld  =  1; 
conf irmServiceConnection . conf irmedRate  = 

createServiceConnection . requestedRate;  //  BUG:  The  actual  rate  may  not  be  the  same  as  the 
request 


conf irmServiceConnection . responseCode  = 


CONNECTION  SUCCESSFUL; 


convertConf irmServiceConnection (data, 
txMsgHeader . dataBytes) ; 


txMsgHeader. cmdCode  =  CONFIRM_SERVICE_CONNECTION; 
txMsgHeader . dstSubsId  =  rxMsgHeader . srcSubsId; 
txMsgHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
txMsgHeader . dstCompId  =  rxMsgHeader . srcCompId; 
txMsgHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
txMsgHeader . dataBytes  = 

&conf irmServiceConnection,  DATASIZE,  PACK) ; 
jmsSend (meepdJms,  StxMsgHeader ,  data. 


meepdScActive  =  TRUE; 


else 


conf irmServiceConnection . cmdCode  = 

createServiceConnection . cmdCode; 

conf irmServiceConnection . instanceld  =  1; 
conf irmServiceConnection . conf irmedRate  = 


createServiceConnection . requestedRate; 

conf irmServiceConnection . responseCode  = 


CONNECTION  REFUSED; 


convertConf irmServiceConnection (data, 
txMsgHeader . dataBytes)  ; 


txMsgHeader . cmdCode  =  CONFIRM_SERVICE_CONNECTION; 
txMsgHeader . dstSubsId  =  rxMsgHeader . srcSubsId; 
txMsgHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
txMsgHeader . dstCompId  =  rxMsgHeader . srcCompId; 
txMsgHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
txMsgHeader . dataBytes  = 

&conf irmServiceConnection,  DATASIZE,  PACK) ; 
jmsSend (meepdJms,  StxMsgHeader ,  data. 


break; 


case  CONFIRM  SERVICE  CONNECTION: 


&conf irmServiceConnection, 
connection  message 


convertConf irmServiceConnection (data, 

DATASIZE,  UNPACK) ; 

//  USER:  Insert  code  here  to  handle  the  confirm  service 


break; 


case 

SactivateServiceConnection, 
connection  message 


ACT I VATE_SERVI CE_CONNECT I ON : 

convertActivateServiceConnection (data, 

DATASIZE,  UNPACK) ; 

//  USER:  Insert  code  here  to  handle  the  activate  service 
break; 


case 

SsuspendServiceConnection, 
connection  message 


SUSPEND_SERVICE_CONNECTION : 

convert SuspendServiceConnect ion (data, 

DATASIZE,  UNPACK) ; 

//  USER:  Insert  code  here  to  handle  the  suspend  service 


break; 


case  TERMINATE  SERVICE  CONNECTION: 


SterminateServiceConnection, 
RE  PORT_ JO I NT_VELOC I T Y ) 


convertTerminateServiceConnection (data, 
DATASIZE,  UNPACK) ; 

if  ( terminateServiceConnection . cmdCode  == 
meepdScActive  =  FALSE; 
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break; 


case  REQUEST_COMPONENT_CONTROL : 

convertRequestComponentControl (data, 
SrequestComponentControl,  DATASIZE,  UNPACK) ; 


if (meepdUnderControl) 


//  Test  for  higher  authority 


REJECT  COMPONENT  CONTROL; 


txMsgHeader . dataBytes)  ; 


if (requestComponentControl  >  meepdCtrlCmptAuthority) 

{ 

//  Terminate  control  of  current  component 
txMsgHeader . cmdCode  = 

txMsgHeader . dstSubsId  =  meepdCtrlCmptSubsId; 
txMsgHeader . dstNodeld  =  meepdCtrlCmptNodeld; 
txMsgHeader . dstCompId  =  meepdCtrlCmptCompId; 
txMsgHeader . dstlnstld  =  meepdCtrlCmptlnstld; 
txMsgHeader . dataBytes  =  0; 
jmsSend (meepdJms,  StxMsgHeader ,  data. 


CONFIRM_COMPONENT_CONTROL; 
rxMsgHeader . srcSubsId; 
rxMsgHeader . srcNodeld; 
rxMsgHeader . srcCompId; 
rxMsgHeader . srclnstld; 

convertConf irmComponentControl (data, 
txMsgHeader . dataBytes)  ; 


requestComponentControl ; 


//  Accept  control  of  new  component 
txMsgHeader . cmdCode  = 

txMsgHeader . dstSubsId  = 

txMsgHeader . dstNodeld  = 

txMsgHeader . dstCompId  = 

txMsgHeader . dstlnstld  = 

conf irmComponentControl . asByte  =  0; 
txMsgHeader . dataBytes  = 

&conf irmComponentControl,  DATASIZE,  PACK) ; 

jmsSend (meepdJms,  StxMsgHeader ,  data, 

meepdCtrlCmptlnstld  =  rxMsgHeader . srclnstld; 
meepdCtrlCmptCompId  =  rxMsgHeader . srcCompId; 
meepdCtrlCmptNodeld  =  rxMsgHeader . srcNodeld; 
meepdCtrlCmptSubsId  =  rxMsgHeader . srcSubsId; 
meepdCtrlCmptAuthority  = 


else 


if  (  rxMsgHeader . srcSubsId  !  = 

meepdCtrlCmptSubsId  | |  rxMsgHeader . srcNodeld  !=  meepdCtrlCmptNodeld  | | 

rxMsgHeader . srcCompId  != 

meepdCtrlCmptCompId  | |  rxMsgHeader . srclnstld  !=  meepdCtrlCmptlnstld  ) 


RE JECT_COMPONENT_CONTROL ; 
rxMsgHeader . srcSubsId; 
rxMsgHeader . srcNodeld; 
rxMsgHeader . srcCompId; 
rxMsgHeader . srclnstld; 

txMsgHeader . dataBytes)  ; 


txMsgHeader . cmdCode  = 

txMsgHeader . dstSubsId  = 

txMsgHeader . dstNodeld  = 

txMsgHeader . dstCompId  = 

txMsgHeader . dstlnstld  = 

txMsgHeader . dataBytes  =  0; 

jmsSend (meepdJms,  StxMsgHeader ,  data. 


control 


else  //  Not  currently  under  component  control,  so  give 
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convertConf irmComponentControl (data, 
txMsgHeader . dataBytes) ; 


txMsgHeader . cmdCode  =  CONFIRM_COMPONENT_CONTROL; 

txMsgHeader . dstSubsId  =  rxMsgHeader . srcSubsId; 
txMsgHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
txMsgHeader . dstCompId  =  rxMsgHeader . srcCompId; 
txMsgHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
conf irmComponentControl . asByte  =  0; 
txMsgHeader . dataBytes  = 

&conf irmComponentControl,  DATASIZE,  PACK) ; 
jmsSend (meepdJms,  StxMsgHeader ,  data, 

meepdCtrlCmptlnstld  =  rxMsgHeader . srclnstld; 
meepdCtrlCmptCompId  =  rxMsgHeader . srcCompId; 
meepdCtrlCmptNodeld  =  rxMsgHeader . srcNodeld; 
meepdCtrlCmptSubsId  =  rxMsgHeader . srcSubsId; 
meepdCtrlCmptAuthority  =  requestComponentControl; 
meepdUnderControl  =  TRUE; 


break; 


case  RELEASE_COMPONENT_CONTROL : 

meepdUnderControl  =  FALSE; 
break; 


case  CONFIRM  COMPONENT  CONTROL: 


&conf irmComponentControl , 
control  message  if  needed 


convertConf irmComponentControl (data, 

DATASIZE,  UNPACK) ; 

//  USER:  Insert  code  here  to  handle  the  confirm  component 


break; 


case 

control  message  if  needed 


RE JECT_COMPONENT_CONTROL : 

//  USER:  Insert  code  here  to  handle  the  reject  component 
break; 


case  QUERY_COMPONENT_AUTHORITY: 

txMsgHeader . cmdCode  =  REPORT_COMPONENT_AUTHORITY; 
txMsgHeader . dstSubsId  =  rxMsgHeader . srcSubsId; 
txMsgHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
txMsgHeader . dstCompId  =  rxMsgHeader . srcCompId; 
txMsgHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
componentAuthority  =  meepdAuthority; 

txMsgHeader . dataBytes  =  convertComponentAuthority (data, 
ScomponentAuthority,  DATASIZE,  PACK) ; 

jmsSend (meepdJms,  StxMsgHeader ,  data, 

txMsgHeader . dataBytes)  ; 


break; 


case  QUERY_COMPONENT_STATUS : 

txMsgHeader . cmdCode  =  REPORT_COMPONENT_STATUS; 
txMsgHeader . dstSubsId  =  rxMsgHeader . srcSubsId; 
txMsgHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
txMsgHeader . dstCompId  =  rxMsgHeader . srcCompId; 
txMsgHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
componentStatus . primaryStatus . asByte  =  meepdState; 
txMsgHeader . dataBytes  =  convertComponentStatus (data, 
ScomponentStatus,  DATASIZE,  PACK) ; 

jmsSend (meepdJms,  StxMsgHeader ,  data, 

txMsgHeader . dataBytes)  ; 


break; 


DATASIZE,  UNPACK) ; 
authority  message  if 


case  REPORT_COMPONENT_AUTHORITY: 

convertComponentAuthority (data,  ScomponentAuthority, 

//  USER:  Insert  code  here  to  handle  the  report  component 

needed 

break; 


case  RE PORT_COMPONENT_S TATU S : 

convertComponentStatus (data,  ScomponentStatus,  DATASIZE, 


UNPACK) ; 
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if ( rxMsgHeader . srcCompId  ==  PRIMITIVE_MANIPULATOR) 
meepdPmState  = 

component Status . primary Status . as Field. primary StatusCode ; 

break; 

case  SET_TOOL_POINT: 

convertToolPoint (data,  &setToolPoint_6,  DATASIZE,  UNPACK); 
break; 


UNPACK) ; 


case  SET_END_EFFECTOR_POSE: 

convertEndEf fectorPose (data,  SsetEndEf fectorPose,  DATASIZE, 
break; 


UNPACK) ; 


case  RE PORT_ JO I NT_E  FFORT : 

convert JointEf fort (data,  &reportedJointEf fort,  DATASIZE, 
break; 


case  RE PORT_MAN I PULATOR^S PEC I F I CAT I ON S : 

convertManipulator Specifications (data, 
&reportedManipulatorSpecif ications ,  DATASIZE,  UNPACK) ; 

break; 


DATASIZE,  UNPACK) ; 


case  REPORT_JOINT_POSITION: 

convert JointPosition (data,  &reportedJointPosition, 

break; 


case  QUERY_TOOL_POINT: 

txMsgHeader . cmdCode  =  REPORT_TOOL_POINT; 
txMsgHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
txMsgHeader . dstCompId  =  rxMsgHeader . srcCompId; 
txMsgHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
txMsgHeader . dstSubsId  =  rxMsgHeader . srcSubsId; 
txMsgHeader . dataBytes  =  convertToolPoint (data, 
&reportedTool Point ,  DATASIZE,  PACK) ; 

jmsSend (meepdJms,  & txMsgHeader ,  data, 

txMsgHeader .dataBytes) ; 

break; 


default : 


break; 


meepdNodemgrThreadRunning  =  FALSE; 
pthread_exit (NULL) ; 

} 

//////////////////////////////////////////////////////////////////////////////////// 
//  Function  used  to  send  a  message  to  the  primitive  manipulator  and  query  for 
//  component  status.  Only  the  primitive  manipulator  component  performs  the  initial 
//  system  checks.  Thus  it  is  essential  that  primitive  manipulator  is  running  and  its 
//  status  is  ready  before  any  other  components  are  started  up. 
//////////////////////////////////////////////////////////////////////////////////// 
void  meepdQueryPmComponentStatus (void) 

{ 

)  rnh  _t  txMsg; 

unsigned  char  data [DATASIZE] ; 
double  requestTimeOutSec  =  0.1; 
static  double  lastRequestTimeSec  =  0; 

//  Setup  Transmit  msg 

txMsg . isExpMsg  =  0; 

txMsg . priority  =  6; 

txMsg. acknCtrl  =  0; 

txMsg . isSvcMsg  =  0; 

txMsg . version  =  1; 

txMsg . reserved  =  0; 

txMsg . srclnstld  =  meepdlnstld; 
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txMsg. srcCompId  =  MANIPULATOR_END_EFFECTOR_POSE_DRIVER; 

txMsg . srcNodeld  =  meepdNodeld; 

txMsg. srcSubsId  =  meepdSubsId; 

txMsg. dataCtrl  =  0; 

txMsg . seqNumber  =  0; 

txMsg. cmdCode  =  QUERY_COMPONENT_STATUS; 
txMsg . dstlnstld  =  1; 

txMsg. dstcompld  =  PRIMITIVEJXLANIPULATOR; 
txMsg . dstNodeld  =  meepdNodeld; 
txMsg. dstSubsId  =  meepdSubsId  ; 
txMsg. dataBytes  =  0; 

if (  (getTimeSeconds ( )  -  lastRequestTimeSec)  >=  requestTimeOutSec) 

{ 

jmsSend (meepdJms,  &txMsg,  data,  txMsg. dataBytes ) ; 
lastRequestTimeSec  =  getTimeSeconds () ; 


//////////////////////////////////////////////////////////////////////////////////// 
//  Function  used  to  set  up  a  message  sent  to  manipulator  joint  position  sensor 
//  to  obtain  current  joint  position  data. 

//////////////////////////////////////////////////////////////////////////////////// 

void  meepdQueryMjps JointPosition (void) 

{ 

j rnh  _t  txMsg; 

unsigned  char  data [DATASIZE] ; 
double  requestTimeOutSec  =  0.1; 
static  double  lastRequestTimeSec  =  0; 

//  Setup  Transmit  msg 

txMsg . isExpMsg  =  0; 

txMsg . priority  =  6; 

txMsg. acknCtrl  =  0; 

txMsg . isSvcMsg  =  0; 

txMsg . version  =  1; 

txMsg . reserved  =  0; 

txMsg. srcSubsId  =  meepdSubsId; 

txMsg . srcNodeld  =  meepdNodeld; 

txMsg. srcCompId  =  MANIPULATOR_END_EFFECTOR_POSE_DRIVER; 
txMsg . srclnstld  =  meepdlnstld; 
txMsg. dataCtrl  =  0; 
txMsg . seqNumber  =  0; 

txMsg. cmdCode  =  QUERY_JOINT_POSITION; 
txMsg . dstlnstld  =  1; 

txMsg. dstcompld  =  MANIPULATOR_JOINT_POSITION_SENSOR; 
txMsg . dstNodeld  =  meepdNodeld; 
txMsg. dstSubsId  =  meepdSubsId  ; 
txMsg. dataBytes  =  0; 

if (  (getTimeSeconds ( )  -  lastRequestTimeSec)  >=  requestTimeOutSec) 

{ 

jmsSend (meepdJms,  &txMsg,  data,  txMsg. dataBytes ) ; 
lastRequestTimeSec  =  getTimeSeconds () ; 

} 


//////////////////////////////////////////////////////////////////////////////////// 
//  Function  used  to  set  up  a  message  sent  to  the  primitive  manipulator  to  obtain 
//  current  effort  values. 

//////////////////////////////////////////////////////////////////////////////////// 

void  meepdQueryPmJointEf f ort (void) 

{ 

jrrih  _t  txMsg; 

unsigned  char  data [DATASIZE] ; 
double  requestTimeOutSec  =  0.1; 
static  double  lastRequestTimeSec  =  0; 

//  Setup  Transmit  msg 
txMsg . isExpMsg  =  0; 
txMsg . priority  =  6; 
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txMsg.acknCtrl  =  0; 

txMsg . isSvcMsg  =  0; 

txMsg . version  =  1; 

txMsg . reserved  =  0; 

txMsg. srcSubsId  =  meepdSubsId; 

txMsg. srcNodeld  =  meepdNodeld; 

txMsg. srcCompId  =  MANIPULATOR_END_EFFECTOR_POSE_DRIVER; 

txMsg . srclnstld  =  meepdlnstld; 

txMsg. dataCtrl  =  0; 

txMsg . seqNumber  =  0; 

txMsg. cmdCode  =  QUERY_JOINT_EFFORT; 

txMsg . dstlnstld  =  1; 

txMsg. dstcompld  =  PRIMITIVEJXLANIPULATOR; 
txMsg . dstNodeld  =  meepdNodeld; 
txMsg. dstSubsId  =  meepdSubsId  ; 
txMsg. dataBytes  =  0; 

if (  (getTimeSeconds ( )  -  lastRequestTimeSec)  >=  requestTimeOutSec) 

{ 

jmsSend (meepdJms,  &txMsg,  data,  txMsg. dataBytes ) ; 
lastRequestTimeSec  =  getTimeSeconds () ; 


//////////////////////////////////////////////////////////////////////////////////// 
//  Function  used  to  set  up  a  message  sent  to  the  primitive  manipulator  to  obtain 
//  manipulator  specifications. 

//////////////////////////////////////////////////////////////////////////////////// 

void  meepdQueryPmManipulatorSpecif ications (void) 

{ 

jirrih  _t  txMsg; 

unsigned  char  data [DATASIZE] ; 
double  requestTimeOutSec  =  0.1; 
static  double  lastRequestTimeSec  =  0; 

//  Setup  Transmit  msg 

txMsg . isExpMsg  =  0; 

txMsg . priority  =  6; 

txMsg.acknCtrl  =  0; 

txMsg . isSvcMsg  =  0; 

txMsg . version  =  1; 

txMsg . reserved  =  0; 

txMsg. srcSubsId  =  meepdSubsId; 

txMsg . srcNodeld  =  meepdNodeld; 

txMsg. srcCompId  =  MANIPULATOR_END_EFFECTOR_POSE_DRIVER; 
txMsg . srclnstld  =  meepdlnstld; 
txMsg. dataCtrl  =  0; 
txMsg . seqNumber  =  0; 

txMsg. cmdCode  =  QUERY_MANIPULATOR_SPECIFICATIONS; 
txMsg . dstlnstld  =  1; 

txMsg. dstcompld  =  PRIMITIVE_MANIPULATOR; 
txMsg . dstNodeld  =  meepdNodeld; 
txMsg. dstSubsId  =  meepdSubsId  ; 
txMsg. dataBytes  =  0; 

if (  (getTimeSeconds ( )  -  lastRequestTimeSec)  >=  requestTimeOutSec) 

{ 

jmsSend (meepdJms,  &txMsg,  data,  txMsg. dataBytes ) ; 
lastRequestTimeSec  =  getTimeSeconds () ; 

} 


//////////////////////////////////////////////////////////////////////////////////// 
//  Function  used  to  set  up  a  message  sent  to  the  primitive  manipulator  with  new 
//  joint  effort  values. 

//////////////////////////////////////////////////////////////////////////////////// 

void  meepdSetPmJointEf f ort (void) 

{ 

j rnh  _t  txMsg; 

double  requestTimeOutSec  =  0.1; 
static  double  lastRequestTimeSec  =  0; 
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unsigned  char  data [DATASIZE] ; 

//Setup  Transmit  Message 

txMsg . isExpMsg  =  0; 

txMsg . priority  =  6; 

txMsg. acknCtrl  =  0; 

txMsg . isSvcMsg  =  0; 

txMsg . version  =  1; 

txMsg . reserved  =  0; 

txMsg . srclnstld  =  meepdlnstld; 

txMsg. srcCompId  =  MANIPULATOR_END_EFFECTOR_POSE_DRIVER; 

txMsg . srcNodeld  =  meepdNodeld; 

txMsg. srcSubsId  =  meepdSubsId; 

txMsg. dataCtrl  =  0; 

txMsg . seqNumber  =  0; 

txMsg. cmdCode  =  SET_JOINT_EFFORT; 

txMsg . dstlnstld  =  1; 

txMsg. dstcompld  =  PRIMITIVEJMANIPULATOR; 
txMsg . dstNodeld  =  meepdNodeld; 
txMsg. dstSubsId  =  meepdSubsId; 

txMsg . dataBytes  =  convert JointEf fort (data,  &set JointEf fort,  DATASIZE,  PACK) ; 

if (  (getTimeSeconds ( )  -  lastRequestTimeSec)  >=  requestTimeOutSec) 

{ 

jmsSend (meepdJms,  &txMsg,  data,  txMsg. dataBytes ) ; 
lastRequestTimeSec  =  getTimeSeconds () ; 

} 


//////////////////////////////////////////////////////////////////////////////////// 
//  Function  used  to  perform  closed  loop  end-effector  position  control 
//////////////////////////////////////////////////////////////////////////////////// 
void  endEf f ectorPoseDriver (double  *cmd_position,  double  *curr_position,  double 
*cmd_ef fort) 

{ 

int  i; 

double  k [NUM_JOINTS] ; 

for  ( i=0 ;  i<NUM_JOINTS ;  i++) 

{ 

if  (i  <  3)  k [ i ]  =34; 
else  if  (i  ==  3)  k[i]  =  34; 
else  if  (i  ==  4)  k[i]  =  15; 
else  k [ i ]  =  14 ; 


if  (: 

L  ==  0) 

cmd  effort | 

:i]  = 

(  (cmd 

position | 

:i] 

-  curr 

position | 

:i] ) 

* 

k  I 

:i] ) 

* 

J1 

_RAD_ 

_T°_ 

ENC 

*  100 

/  112000; 

else 

if  (1  ==  1) 

cmd  effort | 

:i]  = 

(  (cmd 

position | 

:i] 

-  curr 

position | 

:i] ) 

* 

k  I 

:i] ) 

* 

J2_ 

RAD_ 

TO_ 

ENC 

*  100 

/  112000; 

else 

if  (1  ==  2) 

cmd  effort | 

:i]  = 

(  (cmd 

position | 

;i] 

-  curr 

position | 

;i] ) 

* 

k  1 

;i] ) 

* 

J3_ 

_RAD_ 

_T0_ 

ENC 

*  100 

/  112000; 

else 

if  (i  ==  3) 

cmd  effort | 

:i]  = 

(  (cmd 

position | 

:i] 

-  curr 

position | 

:i] ) 

* 

k  | 

:i] ) 

* 

J4 

_RAD_ 

_T0_ 

ENC 

*  100 

/  35000; 

else 

if  (1  ==  4) 

cmd  effort | 

:ij  = 

(  (cmd 

position | 

:i] 

-  curr 

position | 

:i] ) 

* 

k  | 

;i] ) 

* 

J5_ 

RAD_ 

TO_ 

ENC 

*  100 

/  16000; 

else 

cmd  effort | 

:i]  = 

(  (cmd 

position | 

;i] 

-  curr 

position | 

:i] ) 

* 

k  1 

;i] ) 

* 

J6 

RAD 

TO 

ENC 

*  100 

/  15500; 

} 


void  matrixToQuaternion (double  *orientA,  double  *orientS,  double  *curr_orientation) 

{ 

double  T,  S; 
double  W,  X,  Y,  Z; 
double  M0,  Ml,  M2; 
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double  M4,  M5,  M6; 
double  M8,  M9,  M10; 

MO  =  orientA[0]; 

M4  =  orientA[l]; 

M8  =  orientA [2]; 

Ml  =  orients [ 1 ] *orientA [2 ]  -  orients [2 ] *orientA [ 1  ]  ; 

M5  =  orients [2 ] *orientA [ 0 ]  -  orients [ 0 ] *orientA [2 ] ; 
M9  =  orients [ 0 ] *orientA [ 1 ]  -  orients [ 1 ] *orientA [ 0 ] ; 

M2  =  orients [0]  ; 

M6  =  orients [ 1 ] ; 

M10  =  orients [2]; 

T  =  MO  +  M5  +  M10  +  1; 

S  =  0.5/ sqrt (T) ; 

W  =  0.25/S; 

X  =  (M9  -  M6) *S; 

Y  =  (M2  -  M8) *S; 

Z  =  (M4  -  Ml) *S; 


curr 

orientation 

[0] 

=  w 

curr 

orientation 

[1] 

=  X 

curr 

orientation 

[2] 

=  Y 

curr 

orientation 

[3] 

=  Z 

void  quaternionToMatrix (double  *cmd_orientation, double  *des_orientS,  double  *des_orientA) 


double 

W, 

X,  Y, 

.  Z; 

double 

MO, 

,  Ml, 

M2; 

double 

M4, 

,  M5, 

M6  ; 

double 

M8, 

,  M9, 

M10 

W  =  cmd_orientation [ 0 ] ; 
X  =  cmd_orientation [ 1 ] ; 
Y  =  cmd_orientation [2 ] ; 
Z  =  cmd_orientation [3] ; 


MO  : 

=  1  -  2  *  Y 

*  Y 

- 

2*Z* 

M4  : 

=  2*X*Y  + 

2 

*z^ 

*W; 

M8  : 

=  2*X*Z  - 

2 

*  Y- 

*W; 

Ml  ; 

=  2*X*Y  - 

2 

*Z^ 

*W; 

M5  : 

=  1  -  2*X 

*X 

- 

2*Z* 

M9  : 

=  2*Y*Z  + 

2 

*X^ 

*W; 

M2  ; 

=  2*X*Z  + 

2 

*  Y- 

*W; 

M6  : 

=  2*Y*Z  - 

2 

*X^ 

*W; 

M10 

=  1-2* 

X* 

X  - 

-  2  *  Y 

des 

orientA [ 

0] 

= 

MO; 

des 

orientA [ 

1] 

= 

M4  ; 

des 

orientA [ 

2] 

= 

M8  ; 

des 

orients [ 

0] 

= 

M2; 

des 

orients [ 

1] 

= 

M6  ; 

des 

orients [ 

2] 

= 

M10; 

///////////////////////////////////////////////////////////////////////////////////////// 

//  File:  meepd.h 

//  Version:  0.1  Original  Creation 

//  Written  by:  Ognjen  Sosa  (ognjensosa@hotmail.com) 

//  Template  by:  Tom  Galluzzo  (galluzzt@ufl.edu) 
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//  Date:  09/28/2004 

1 1 

//  Description:  This  file  contains  the  skeleton  C  header  file  code  for  implementing 

//  the  meepd.c  file 

///////////////////////////////////////////////////////////////////////////////////////// 

#ifndef  MEEPD_H 
#def ine  MEEPD_H 

#include  "jaus.h" 

#ifndef  FALSE 
#def ine  FALSE  0 
#endif 

#ifndef  TRUE 
#define  TRUE  1 
#endif 

#def ine  MEEP D_NODE_MANAGER_CHECK I N_ERROR 
#def ine  MEEPD_JMS_OPEN_ERROR 

#def ine  MEEPD_STARTUP_BEFORE_SHUTDOWN_ERROR  -3 
#de  f i ne  MEE  P  D_S  TATE_THREAD_CREATE_ERROR 
#def ine  MEEPD_NODEMGR_THREAD_CREATE_ERROR  -5 

#def ine  MEEPD_THREAD_TIMEOUT_SEC  1.0 

//  Public 

int  meepdStartup (void) ; 
int  meepdShutdown (void) ; 
int  meepdGetState (void) ; 
unsigned  char  meepdGetlnstanceld ( void) ; 
unsigned  char  meepdGetCompId ( void) ; 
unsigned  char  meepdGetNodeld ( void) ; 
unsigned  char  meepdGetSubsystemld ( void) ; 
double  meepdGetUpdateRate (void) ; 
int  meepdGetPmState (void) ; 

//  Query  messages  requested  by  this  component 
void  meepdQueryPmComponentStatus (void) ; 
void  meepdQueryPmJointEf f ort (void) ; 
void  meepdQueryMjps JointPosition (void) ; 
void  meepdQueryPmManipulatorSpecif ications (void) ; 

//  Set  messages  that  this  component  commands 
void  meepdSetPmJointEf f ort (void) ; 

//  Accessors  to  incoming  information 
toolPoint_t  *  meepdSetToolPoint (void) ; 
endEf f ectorPose_t  *  meepdSetEndEf fectorPose (void) ; 
jointEf fort_t  *  meepdReport JointEf f ort (void) ; 

manipulatorSpecif ications_t  *  meepdReportManipulatorSpecif ications (void) ; 
jointPosition_t  *  meepdReport JointPosition (void) ; 

//  Accessors  to  outgoing  information 
toolPoint_t  *  meepdReportToolPoint (void) ; 
jointEf fort_t  *  meepdSet JointEf fort (void)  ; 

double  *  meepdGetCurrentOrientation (void) ; 

void  endEf fectorPoseDriver (double  *,  double  *,  double  *); 
void  matrixToQuaternion (double  *,  double  *,  double  *); 
void  quaternionToMatrix (double  *,  double  *,  double  *); 

#endif  //  MEEPD  H 


B.5  The  Meedpd.c  File  and  the  Corresponding  Header  Meedpd.h 


//  File: 


meedpd.c 
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//  Version:  0.1  Original  Creation 

//  Written  by:  Ognjen  Sosa  (ognjensosa@hotmail.com) 


Template  by: 
Date : 


// 

// 

// 

//  Description: 

// 


Tom  Galluzzo  (galluzzt@ufl.edu) 

09/28/2004 

This  file  contains  the  C  code  for  implementation  of  a  Manipulator 
End-Effector  Discrete  Pose  Driver  JAUS  component  in  a  Linux 
environment.  This  code  is  designed  to  work  with  the  node  manager 
and  JAUS  library  software  written  by  Jeff  Witt 


// 

// 

///////////////////////////////////////////////////////////////////////////////////////// 


//  JAUS  message  set  (USER:  JAUS  libraries 


//  Multi-threading  functions  (standard  to  unix) 
//  Precise  timing  routines  (USER:  must  link 


#include  <jaus.h> 
must  be  installed  first) 

#include  <pthread.h> 

#include  <timeLib.h> 
timeLib.c,  written  by  Tom  Galluzzo) 

#include  <unistd.h>  //  Unix  standard  functions 

#include  <stdio.h>  //  Unix  standard  input-output 

#include  <math.h>  //  Mathematical  functions 

#include  Cnodemgr/nodemgr . h>  //  Node  managment  functions  for  sending  and  receiving  JAUS 
messages  (USER:  Node  Manager  must  be  installed) 


#include  "meedpd.h"  //  USER:  Implement  and  rename  this  header  file.  Include  prototypes 
for  all  public  functions  contained  in  this  file. 

#include  "mcConstants.h" 

#include  "galillnterface . h"  //  Access  to  setMaxSpeed ( )  function 
#include  "cpplnterface . h" 

#include  "logLib.h" 

#define  DATASIZE  JDATASIZE  //  Default  size  for  byte  stream  buffers  in  this  file 


//  Private  function  prototypes 
void  *meedpdStateThread (void  *) ; 
void  *meedpdNodemgrThread (void  *) ; 


//  Query  messages  requested  by  this  component 

void  meedpdQueryPmComponentStatus (void) ; 

void  meedpdQueryPmJointEf fort (void) ; 

void  meedpdQueryMjps JointPosition (void) ; 

void  meedpdQueryPmManipulatorSpecif ications (void) ; 

//  Set  messages  that  this  component  commands 
void  meedpdSetPmJointEf fort (void) ; 


//  Accessors  to  incoming  information 
toolPoint_t  *  meedpdSetToolPoint (void) ; 

endEf fectorPathMotion_t  *  meedpdSetEndEf fectorPathMotion (void) ; 
jointEf fort_t  *  meedpdReport JointEf fort (void) ; 

manipulatorSpecif ications_t  *  meedpdReportManipulatorSpecif ications (void) ; 
jointPosition_t  *  meedpdReport JointPosition (void) ; 

//  Accessors  to  outgoing  information 
toolPoint_t  *  meedpdReportToolPoint (void) ; 
jointEf fort_t  *  meedpdSet JointEf fort (void) ; 

//  Accessots  to  other  information 

double  *  meedpdGetCurrentOrientation (void) ; 

int  meedpdGetPose (void) ; 

double  meedpdGetPoseTime (void) ; 

double  meedpdGetCurrentTime (void) ; 

double  *  meedpdGetCurrentEndEf fectorPosition (void) ; 
double  *  meedpdGetCurrentEndEf fectorOrientation (void) ; 


unsigned  char  meedpdlnstld,  meedpdNodeld,  meedpdSubsId; 
and  Subsystem  IDs  for  this  component 

//  JAUS  Instance, 

Node 

componentAuthority  t  meedpdAuthority  =  0; 

Authority  for  this  component 

//  JAUS 

int  meedpdState  =  SHUTDOWN  STATE; 

State 

// 

JAUS 

double  meedpdThreadHz ; 

Stores  the  calculated  update  rate  for  main  state  thread 

// 
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unsigned  char  meedpdCtrlCmptlnstld,  meedpdCtrlCmptCompId; 
component  currently  in  control 

unsigned  char  meedpdCtrlCmptNodeld,  meedpdCtrlCmptSubsId; 

component  currently  in  control 

unsigned  char  meedpdCtrlCmptAuthority; 

authority  level  of  the  component  currently  in  control 

int  meedpdUnderControl  =  FALSE; 

FALSE  -  not  under  control,  TRUE  is  under  control 
int  meedpdRun  =  FALSE; 


// 

Stores 

identity 

of 

the 

// 

Stores 

identity 

of 

the 

//  Stores  the 
// 


// 

// 

//  Structure  storing  incoming  request  under  case  SET  TOOL  POINT 
static  toolPoint_t  setToolPoint_6; 

//  Structure  storing  incoming  request  under  case  SET  END  EFFECTOR  DISCRETE_POSE 
static  endEf fectorPathMotion  t  setEndEf fectorPathMotion; 


int  meedpdStateThreadRunning  =  FALSE; 
pthread_t  meedpdStateThreadld; 
pthread  node  manager  thread  identifier 
int  meedpdNodemgrThreadRunning  =  FALSE; 
pthread_t  meedpdNodemgrThreadld; 
pthread  node  manager  thread  identifier 


//  Structure  storing  incoming  request  under  case  QUERY_TOOL_POINT 
static  toolPoint_t  reportedToolPoint ; 

//  Structures  storing  queried  information 
static  jointEf fort_t  reportedJointEf fort ; 

static  manipulator Specif ications_t  reportedManipulatorSpecif i cat ions; 
static  jointPosition_t  reportedJointPosition; 

//  Structure  storing  outgoing  request  to  the  PRIMITIVE  MANIPULATOR 
static  jointEf fort_t  set JointEf fort ; 

static  double  max_speed [NUM_JOINTS] ,  max_acceleration [NUM_JOINTS] , 

max_deceleration [NUM_JOINTS] ; 

static  double  cmd_pos it ion [NUM_ JOINTS ] ; 

static  int  num_soln; 

static  double  orients [3],  orientA[3]; 

static  double  des_position [3] ,  des_orientS [3] ,  des_orientA [3] ,  curr_orientation [ 4 ] , 
cmd_orientation [4] ; 
static  int  pose,  numPoses; 

static  double  ref_time  =  0,  rel_time  =  0,  init_pose_time  =  0,  pose_time  =  0; 
jmh_t  meedpdScHeader ; 

jms_t  meedpdJms;  //  An  accessor  to  the  Node  Manager  JAUS  Message  Service  (Jms)  for 

this  component 

int  meedpdScActive  =  FALSE; 

int  meedpdPmState  =  -1; 
unsigned  char  meedpdPmNodeld  =  0; 
int  meedpdPmControl  =  FALSE; 

//  Function:  meedpdStartup 

//  Access:  Public 

//  Description:  This  function  allows  the  abstracted  component  functionality  contained  in 
this  file  to  be  started  from  an  external  source. 

//  It  must  be  called  first  before  the  component  state  machine 

and  node  manager  interaction  will  begin 

//  Each  call  to  "meedpdStartup"  should  be  followed  by  one  call 

to  the  "cmptShutdown"  function 
int  meedpdStartup (void) 

{ 

pthread_attr_t  attr;  //  Thread  attributed  for  the  component  threads  spawned  in 
this  function 


ref_time  =  getTimeSeconds ( ) ; 

if (meedpdState  ==  SHUTDOWN_STATE)  //  Execute  the  startup  routines  only  if  the 

component  is  not  running 

{ 
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IDs 

Smeedpdlnstld, 
define  on  this 


//  Check  in  to  the  Node  Manager  and  obtain  Instance,  Node  and  Subsystem 

if ( jcmCheckln (MANIPULATOR_END_EFFECTOR_DISCRETE_POSE_DRIVER, 
SmeedpdNodeld,  SmeedpdSubsId)  <  0)  //  USER:  Insert  your  component  ID 
line 


cError ( "meedpd :  Could  not  check  in  to  nodemgr\n") ; 
return  MEEDPD  NODE  MANAGER  CHECKIN  ERROR; 


//  Open  a  conection  to  the  Node  Manager  and  obtain  a  Jms  accessor 
if ( (meedpdJms  =  jmsOpen (MANIPULATOR_END_EFFECTOR_DISCRETE_POSE_DRIVER, 
meedpdlnstld,  0) )  ==  NULL)  //  USER:  Insert  your  component  ID  define  on  this  line 
{ 

cError ( "meedpd :  Could  not  open  connection  to  nodemgr\n"); 
j  cmCheckOut (MANIPULATOR_END_EFFECTOR_DISCRETE_POSE_DRIVER, 
meedpdlnstld) ;  //  USER:  Insert  your  component  ID  define  on  this  line 

return  MEEDPD_JMS_OPEN_ERROR; 

} 


meedpdJms->timeout . tv_usec  =  100000;  //  Timeout  for  non-blocking  jmsRecv, 
specified  in  micro  seconds 

pthread_attr_init (&attr)  ; 

pthread_attr_setdetachstate ( &attr ,  PTHREAD_CREATE_DETACHED) ; 

meedpdState  =  INITIALIZE_STATE;  //  Set  the  state  of  the  JAUS  state  machine 

to  INITIALIZE 

meedpdRun  =  TRUE; 

if (pthread_create ( SmeedpdStateThreadld,  &attr,  meedpdStateThread,  NULL)  != 


cError ( "meedpd:  Could  not  create  meedpdStateThread\n" ) ; 
meedpdShutdown ( ) ; 

return  MEEDPD_STATE_THREAD_CREATE_ERROR; 

} 

if (pthread_create ( SmeedpdNodemgrThreadld,  &attr,  meedpdNodemgrThread, 

NULL)  ! =  0) 

{ 

cError ( "meedpd :  Could  not  create  meedpdNodemgrThread\n" ) ; 
meedpdShutdown ( ) ; 

return  MEEDPD_NODEMGR_THREAD_CREATE_ERROR; 

} 

pthread_attr_destroy ( &attr)  ; 

} 

else 

{ 

cError ( "meedpd :  Attempted  startup  while  not  shutdown\n" ) ; 
return  ME E D P D_S T ARTU P_BE FORE_S HUT DOWN_E RROR ; 

} 


return 


0; 


//  Function:  meedpdShutdown 
//  Access:  Public 

//  Description:  This  function  allows  the  abstracted  component  functionality 

contained  in  this  file  to  be  stoped  from  an  external  source. 

//  If  the  component  is  in  the  running  state,  this  function 

will  terminate  all  threads  running  in  this  file 

//  This  function  will  also  close  the  Jms  connection  to  the 

Node  Manager  and  check  out  the  component  from  the  Node  Manager 
int  meedpdShutdown (void) 

{ 

double  timeOutSec; 

if (meedpdState  !=  SHUTDOWN_STATE)  //  Execute  the  startup  routines  only  if  the 

component  is  not  running 
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meedpdRun  =  FALSE ; 

timeOutSec  =  getTimeSeconds ( )  +  MEEDPD_THREAD_TIMEOUT_SEC; 
while (meedpdStateThreadRunning) 

{ 

usleep (100000) ; 

if (getTimeSeconds ( )  >=  timeOutSec) 


pthread_cancel (meedpdStateThreadld)  ; 
meedpdStateThreadRunning  =  FALSE; 

cError ( "meedpd :  meedpdStateThread  Shutdown  Improperly\n" ) 
break; 

} 

} 


timeOutSec  =  getTimeSeconds ( )  +  MEEDPD_THREAD_TIMEOUT_SEC; 
while (meedpdNodemgrThreadRunning) 

{ 

usleep (100000) ; 

if (getTimeSeconds ( )  >=  timeOutSec) 


Improperly\n" )  ; 

} 


pthread_cancel (meedpdNodemgrThreadld)  ; 

meedpdNodemgrThreadRunning  =  FALSE; 

cError ( "meedpd:  meedpdNodemgrThread  Shutdown 

break; 


//  Close  Node  Manager  Connection  and  check  out 
jmsClose (meedpdJms) ; 

j  cmCheckOut (MANIPULATOR_END_EFFECTOR_DISCRETE_POSE_DRIVER,  meedpdlnstld) 
//  USER:  Insert  your  component  ID  define  on  this  line 

meedpdState  =  SHUTDOWN_STATE; 

} 


return 


0; 


//  The  series  of  functions  below  allow  public  access  to  essential  component  information 

//  Access:  Public  (All) 

int  meedpdGetState (void) {  return  meedpdState;  } 

unsigned  char  meedpdGetlnstanceld ( void) {  return  meedpdlnstld;  } 
unsigned  char  meedpdGetCompId ( void) {  return  (unsigned 
char) MANIPULATOR_END_EFFECTOR_DISCRETE_POSE_DRIVER;  } 
unsigned  char  meedpdGetNodeld ( void) {  return  meedpdNodeld;  } 
unsigned  char  meedpdGetSubsystemld ( void) {  return  meedpdSubsId;  } 
double  meedpdGetUpdateRate (void) {  return  meedpdThreadHz;  } 
int  meedpdGetPmState (void) {  return  meedpdPmState;  } 

//  USER:  Insert  any  additional  public  variable  accessors  here 
toolPoint_t  *  meedpdSetToolPoint (void)  {return  &setToolPoint_6 ;  } 
endEf f ectorPathMotion_t  *  meedpdSetEndEf fectorPathMotion (void)  {return 
SsetEndEf fectorPathMotion;  } 

jointEf fort_t  *  meedpdReport JointEf fort (void)  {  return  &reportedJointEf fort;  } 
manipulatorSpecif ications_t  *  meedpdReportPmManipulatorSpecif ications (void)  {  return 
&reportedManipulator Specifications;  } 

jointPosition_t  *  meedpdReport JointPosition (void)  {  return  &reportedJointPosition;  } 
toolPoint_t  *  meedpdReportToolPoint (void)  {  return  &reportedToolPoint;  } 
jointEf fort_t  *  meedpdSet JointEf fort (void)  {  return  &set JointEf fort;  } 

double  *  meedpdGetCurrentOrientation (void)  {  return  curr_orientation;  } 
int  meedpdGetPose (void)  {  return  pose;  } 
double  meedpdGetPoseTime (void)  {return  pose_time;  } 
double  meedpdGetCurrentTime (void)  {return  rel_time;  } 

double  *  meedpdGetCurrentEndEf f ectorPosition (void)  {return  des_position;  } 
double  *  meedpdGetCurrentEndEf fectorOrientation (void)  {return  cmd_orientation;  } 

//  Function:  meedpdThread 
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//  Access: 

//  Description: 

// 

here . 

void  *meedpdStateThread (void  ’ 

{ 

jmh_t  txMsgHeader; 
double  time,  prevTime; 
unsigned  char  data [DATASIZE] ; 
int  i,  j; 

double  curr_position [NUM_JOINTS ] 
cur r_tool_point [ 3 ] ,  tool_point_6 [ 3 ] 
double  spec_a [NUM_J0INTS-1 ]  , 
meedpdStateThreadRunning  = 

//  Setup  transmit  header 
txMsgHeader . isExpMsg  =  0; 
if  experimental 

txMsgHeader . isSvcMsg  =  0; 
txMsgHeader . acknCtrl  =  0; 
txMsgHeader . priority  =  6; 
txMsgHeader . reserved  =  0; 
txMsgHeader . version  =  1; 
txMsgHeader . srclnstld 
txMsgHeader . srcCompId 
txMsgHeader . srcNodeld  =  meedpdNodeld; 
txMsgHeader . srcSubsId  =  meedpdSubsId; 
txMsgHeader . dataCtrl  =  0; 
txMsgHeader . seqNumber  =  0; 


_alpha [NUM_ JOINTS -1 ] ,  spec_S [NUM_JOINTS] ; 
// 


//  not  experimental  msg  -  USER:  change  to  1 

//  not  a  service  connection 
//  no  ack/nak 
//  default  priority 


//  JAUS  3 . Ou 

=  meedpdlnstld; 

=  MANIPULATOR  END  EFFECTOR  DISCRETE  POSE  DRIVER; 


Private 

All  core  component  functionality  is  contained  in  this  thread. 

All  of  the  JAUS  component  state  machine  code  can  be  found 

threadData) 


curr_ef f ort [NUM_JOINTS] ,  cmd_effort [NUM_JOINTS] 
cmd_position [3] ; 
spec 
TRUE  ; 


time  =  getTimeSeconds ( ) ; 

meedpdState  =  INITIALIZE_STATE;  //  Set  JAUS  state  to  INITIALIZE 

while (meedpdRun)  //  Execute  state  machine  code  while  not  in  the  SHUTDOWN  state 

{ 

prevTime  =  time; 

time  =  getTimeSeconds () ; 

meedpdThreadHz  =  1.0/ (time-prevTime) ;  //  Compute  the  update  rate  of  this 

thread 

switch (meedpdState)  //  Switch  component  behavior  based  on  which  state  the 

machine  is  in 


case  INITIALIZE_STATE: 

//Check  the  status  of  PRIMITIVE  MANIPULATOR 
meedpdQueryPmComponentStatus ( ) ; 
if (meedpdPmState  ==  EMERGENCY_STATE) 

{ 

cError ( "meedpd :  PM  in  emergency  state,  switching  to 

emergency  state") ; 

meedpdState  =  EMERGENCY_STATE; 


if (meedpdPmState  ==  STANDBY_STATE) 

{ 

txMsgHeader . cmdCode  =  RESUME; 
txMsgHeader . dstlnstld  =  1; 

txMsgHeader .dstCompId  =  PRIMITIVE_MANIPULATOR; 
txMsgHeader . dstNodeld  =  meedpdNodeld; 
txMsgHeader . dstSubsId  =  meedpdSubsId  ; 
txMsgHeader . dataBytes  =  0; 
jmsSend (meedpdJms,  StxMsgHeader ,  data, 

txMsgHeader .dataBytes)  ; 


if (meedpdPmState  ==  READ Y_S  TATE ) 

meedpdState  =  STANDBY_STATE; 

break; 

case  STANDBY_STATE: 

ref_time  =  getTimeSeconds () ; 
rel_time  =  getTimeSeconds))  - 
pose  =0,  j  =  0; 


ref  time; 
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break; 


values 


case  READY_STATE: 

//  Set  up  default  velocity,  acceleration,  and  deceleration 
for  (1=0;  i<NUM_JOINTS;  i++) 


if  (i  ==  0) { 


max_speed[i]  =  5000  / 

Jl_RAD_TO_ENC; //reportedManipulatorSpecif ications . j ointSpecif ications [i] . j ointMaximumVelo 
city  /  1000; 

max_acceleration [i]  =  128000  / 

Jl_RAD_TO_ENC; 

} 

else  if  (i  ==  1)  { 

max_speed[i]  =  5000  /  J2_RAD_TO_ENC; 
max_acceleration [i]  =  128000  / 

J2  RAD  TO  ENC; 


J3  RAD  TO  ENC; 


J4  RAD  TO  ENC; 


J5  RAD  TO  ENC; 


else  if  (i  ==  2)  { 

max_speed[i]  =  5000  /  J3_RAD_TO_ENC; 
max_acceleration [i]  =  128000  / 


} 

else  if  (i  ==  3)  { 

max_speed[i]  =  1000 
max_acceleration [ i ] 


/  J4_RAD_TO_ENC; 
=  128000  / 


} 

else  if  (i  ==  4)  { 

max_speed[i]  =  1000 
max_acceleration [ i ] 


/  J5_RAD_TO_ENC ; 
=  128000  / 


else  { 

max_speed[i]  =  1000  / 

J6_RAD_TO_ENC; //reportedManipulatorSpecif ications . j ointNmaximumVelocity  /  1000; 

max_acceleration [i]  =  128000  / 


J6  RAD  TO  ENC; 


emergency  state"); 


max_deceleration [ i ]  =  max_acceleration [i] ; 

} 

setMaxSpeed (max_speed) ; 
setMaxAcceleration (max_acceleration) ; 
setMaxDeceleration (max_deceleration)  ; 

//Check  the  status  of  PRIMITIVE  MANIPULATOR 
meedpdQueryPmComponentStatus ( ) ; 
if (meedpdPmState  ==  EMERGENCY_STATE) 

{ 

cError ( "meedpd :  PM  in  emergency  state,  switching  to 
meedpdState  =  EMERGENCY'S TATE; 

} 


numPoses  =  setEndEf f ectorPathMotion . numPoses ; 

//  Start  timer 

rel_time  =  getTimeSeconds ( )  -  ref_time; 
init_pose_time  = 

setEndEf f ectorPathMotion . endEf fectorPoses [0] . time; 

pose_time  = 

setEndEf f ectorPathMotion . endEf fectorPoses [ j ] . time; 

//  Set  zero  efforts  until  first  motion  profle  comes  in 
if  (rel_time  <=  init_pose_time) 

{ 

pose  =  0; 

//  Set  Joint  Efforts  to  zero 

set JointEf fort . numjoints  =  NUM  JOINTS; 
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for  ( i=0 ;  i<NUM_JOINTS ;  i++) 

{ 

set JointEf fort . jointEf fort [i]  =  0; 

} 

meedpdSetPmJointEf f ort ( )  ; 


//  Query  for  required  information 
meedpdQueryMjps JointPosition ( )  ; 
meedpdQueryPmManipulatorSpecif ications ()  ; 
meedpdQueryPmJointEf f ort ()  ; 

for  ( i=0 ;  i<NUM_JOINTS ;  i++) 

{ 

curr_position [i]  = 

reported Joint Posit ion . j oint Posit ion [ i ]  ; 

curr_effort [i]  =  reportedJointEf fort . j ointEf fort [ i ] 

} 


for ( i=0 ;  i<NUM_JOINTS-l ;  i++) 

{ 

spec_a[i]  = 

reportedManipulator Specif ications . j  oint Specif ications [ i ]  . linkLength; 

spec_alpha [i]  = 

reportedManipulator Specif ications . j oint Specif ications [i] . twistAngle/1000; 

if  ( i<3  |  |  i — 4> 

spec_S[i]  = 

reportedManipulatorSpecif ications . j ointSpecif ications [i] . jointOffsetOrAngle; 

else  if  (i==3) 

spec_S[i]  =  - 

(reportedManipulatorSpecif ications . j ointSpecif ications [i]  . jointOffsetOrAngle) ; 

} 

spec_S[5]  =  - 

(reportedManipulatorSpecif ications . jointNoffsetOrAngle)  ; 


//  Extract  incoming  tool  point  in  6th  coordinate 
tool_point_6 [0]  =  0;  //  mm 
tool_point_6 [ 1 ]  =  10; 
tool_point_6 [2 ]  =  0; 


orientation  of  the 
tool_point_6,  curr 
orients,  orientA) ; 


//  This  function  returns  the  current  position  and 
end-effector 

getCurrentEndEf fectorPose (spec_a,  spec_alpha,  spec_S, 

position. 


curr_tool_point. 


PmatrixToQuaternion (orientA,  orients,  curr_orientation) ; 
//  Report  Tool  Point 

reportedToolPoint.X  =  curr_tool_point [ 0 ] ; 
reportedToolPoint . Y  =  curr_tool_point [ 1 ] ; 
reportedToolPoint .  Z  =  curr_tool__point  [2  ]  ; 

if  (rel_time  >=  pose_time) 


orientation 


//  Extract  information  about  desired  position  and 


des_position [0]  = 

setEndEf f ectorPathMotion . endEf f ectorPoses [ j ] . X  *  1000;  //  mm 

des_position [ 1 ]  = 

setEndEf f ectorPathMotion . endEf f ectorPoses [ j ]. Y  *  1000; 

des_position [2 ]  = 

setEndEf f ectorPathMotion . endEf f ectorPoses [ j ]. Z  *  1000; 

cmd_or ientation [ 0 ]  = 

setEndEf f ectorPathMotion . endEf f ectorPoses [ j ] . quaternionQcomponentD; 

cmd_orientation [ 1 ]  = 

setEndEf f ectorPathMotion . endEf f ectorPoses [ j ] . quaternionQcomponentA; 

cmd_or ientation [ 2 ]  = 

setEndEf f ectorPathMotion . endEf f ectorPoses [ j ] . quaternionQcomponentB; 
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cmd_or ientation [ 3 ]  = 

setEndEf f ectorPathMotion . endEf f ector Poses [ j ] . quaternionQcomponentC; 

PquaternionToMatrix (cmd_orientation,  des_orientS, 

des_orientA) ; 


corresponding  to  the  desired  pose 
des_orientA,  cmd_position) ; 


//  This  function  returns  the  joint  angles 

getDesiredJointAngles (des_position,  des_orientS, 

if  (j  <  numPoses-1) { 
pose+t; 
j++; 

} 

else  { 

j  =  numPoses-1; 
pose  =  numPoses; 

} 


control 
cmd_effort) ; 


//  This  function  performs  closed  loop  end  effector  pose 
endEf fectorDiscretePoseDriver (cmd_position,  curr_position, 


//  Set  Joint  Efforts 

set JointEf fort . numjoints  =  NUM_JOINTS; 
for  ( 1=0 ;  i<NUM_JOINTS;  i++) 

{ 

set JointEf fort . jointEf fort [i]  =  cmd_effort [i] ; 

} 

meedpdSetPmJointEf f ort ( ) ; 
break; 


case  EMERGENCY_STATE: 
break; 

case  FAILURE_STATE: 
break; 

case  SHUTDOWN_STATE: 
break; 

default : 

meedpdState  =  FAILURE_STATE;  //  The  default  case  is 
undefined,  therefore  go  into  Failure  State 

break; 

} 

usleep (1000) ;  //  Sleep  for  one  millisecond 


//  Sleep  for  50  milliseconds  and  then  exit 
usleep (50000) ; 

meedpdStateThreadRunning  =  FALSE; 
pthread_exit (NULL) ; 

} 


//  Function:  meedpdNodemgrThread 
//  Access:  Private 

//  Description:  This  thread  is  responsible  for  handling  incoming  JAUS  messages  from 

the  Node  Manager  JAUS  message  service  (Jms) 

//  incoming  messages  are  processed  according  to  message  type, 

void  *meedpdNodemgrThread ( void  *threadData) 

{ 

jmh_t  rxMsgHeader,  txMsgHeader;  //  Declare  JAUS  header  data  structures,  where  the 
jmh_t  data  structure  is  defined  in  jaus.h 
unsigned  char  data [DATASIZE] ; 

createServiceConnection  t  createServiceConnection; 
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conf irmServiceConnection_t  conf irmServiceConnection; 
activateServiceConnection_t  activateServiceConnection; 
suspendServiceConnection_t  suspendServiceConnection; 
terminateServiceConnection_t  terminateServiceConnection; 
requestComponentControl_t  requestComponentControl ; 
conf irmComponentControl_t  conf irmComponentControl; 
component Author ity_t  componentAuthority; 
componentStatus_t  componentStatus ; 
int  recvCount; 


meedpdNodemgrThreadRunning  =  TRUE;  // 


//  Setup  transmit  header 
txMsgHeader . isExpMsg  =  0; 
if  experimental 

txMsgHeader . isSvcMsg  =  0; 
txMsgHeader . acknCtrl  =  0; 
txMsgHeader . priority  =  6; 
txMsgHeader . reserved  =  0; 
txMsgHeader . version  = 
txMsgHeader . srclnstld 
txMsgHeader . srcCompId 
txMsgHeader . srcNodeld  =  meedpdNodeld; 
txMsgHeader . srcSubsId  =  meedpdSubsId; 
txMsgHeader . dataCtrl  =  0; 
txMsgHeader . seqNumber  =  0; 


//  not  experimental  msg  -  USER:  change  to  1 

//  not  a  service  connection 
//  no  ack/nak 
//  default  priority 


1;  //  JAUS  3.0 

=  meedpdlnstld; 

=  MANIPULATOR  END  EFFECTOR  DISCRETE  POSE  DRIVER; 


while (meedpdRun)  //  Execute  incoming  JAUS  message  processing  while  not  in  the 
SHUTDOWN  state 


//  Check  for  a  new  message  in  the  Node  Manager  JMS  queue 
//  If  a  message  is  present  then  store  the  header  information  in 
rxMsgHeader,  and  store  the  message  content  information  in  the  data  buffer 

recvCount  =  jmsRecv (meedpdJms,  SrxMsgHeader ,  data,  DATASIZE) ; 
if (recvCount  <  0) 


recvCount) ; 


cError ( "meedpd :  Node  manager  jmsRecv  returned  error:  %d\n", 
break; 


if (recvCount  ==  0)  continue; 

//  This  block  of  code  is  intended  to  reject  commands  from  non-controlling 

components 

//  However,  the  one  exception  allowed  is  a  REQUEST_COMPONENT_CONTROL 
if (meedpdUnderControl  &&  rxMsgHeader . cmdCode  <  0x2000)  //  If  not  currently 
under  control  or  it's  not  a  command,  then  go  to  the  switch  statement 


//  If  the  source  component  isn't  the  one  in  control,  and  it's  not  a 
request  to  gain  control,  then  exit  this  iteration  of  the  while  loop 

if (  !(  meedpdCtrlCmptInstId==rxMsgHeader . srclnstld  && 

meedpdCtrlCmptCompId==rxMsgHeader . srcCompId  && 
meedpdCtrlCmptNodeId==rxMsgHeader . srcNodeld  && 
meedpdCtrlCmptSubsId==rxMsgHeader . srcSubsId) && 

!  (  rxMsgHeader . cmdCode==REQUEST_COMPONENT_CONTROL) 

) continue;  //  to  next  iteration 

} 


switch (rxMsgHeader . cmdCode)  //  Switch  the  processing  algorithm  according 
to  the  JAUS  message  type 


code 


UNPACK)  ; 


//  Set  the  component  authority  according  to  the  incoming  authority 
case  SET_COMPONENT_AUTHORITY : 

convertComponentAuthority (data,  SmeedpdAuthority,  DATASIZE, 
//  Unpack  and  store  the  incoming  authority  code 
break; 


case  SHUTDOWN: 

meedpdState  =  S HUT DOWN_S TATE ; 
break; 
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case  STANDBY: 

if (meedpdState  ==  READY_STATE) 

meedpdState  =  STANDBY_STATE; 

break; 

case  RESUME: 

if (meedpdState  ==  STANDBY_STATE ) 

meedpdState  =  READY_STATE; 

break; 
case  RESET: 

meedpdState  =  INITIALIZE_STATE; 
break; 

case  SET_EMERGENCY : 

meedpdState  =  EMERGENCY_STATE; 
break; 

case  CLEAR_EMERGENCY : 

meedpdState  =  STANDBY_STATE; 
break; 


case  CREATE_SERVICE_CONNECTION : 

convertCreateServiceConnection (data, 
ScreateServiceConnection,  DATASIZE,  UNPACK) ; 

if (createServiceConnection . cmdCode  == 


REPORT  JOINT  VELOCITY) 


meedpdScHeader  =  txMsgHeader; 
meedpdScHeader . isSvcMsg  =  1; 

meedpdScHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
meedpdScHeader . dstCompId  =  rxMsgHeader . srcCompId; 
meedpdScHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
meedpdScHeader . dstSubsId  =  rxMsgHeader . srcSubsId; 
meedpdScHeader . cmdCode  =  REPORT_JOINT_VELOCITY; 
meedpdScHeader . seqNumber  =  1; 


conf irmServiceConnection . cmdCode  = 


RE  PORT_ JO I NT_VELOC I T Y ; 

conf irmServiceConnection . instanceld  =  1; 
conf irmServiceConnection . conf irmedRate  = 

createServiceConnection . requestedRate;  //  BUG:  The  actual  rate  may  not  be  the  same  as  the 
request 


conf irmServiceConnection . responseCode  = 


CONNECTION  SUCCESSFUL; 


convertConf irmServiceConnection (data, 
txMsgHeader . dataBytes)  ; 


txMsgHeader . cmdCode  =  CONFIRM_SERVICE_CONNECTION; 
txMsgHeader . dstSubsId  =  rxMsgHeader . srcSubsId; 
txMsgHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
txMsgHeader . dstCompId  =  rxMsgHeader . srcCompId; 
txMsgHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
txMsgHeader . dataBytes  = 

&conf irmServiceConnection,  DATASIZE,  PACK) ; 
jmsSend (meedpdJms,  StxMsgHeader ,  data. 


meedpdScActive  =  TRUE; 


else 


conf irmServiceConnection . cmdCode  = 

createServiceConnection . cmdCode; 

conf irmServiceConnection . instanceld  =  1; 
conf irmServiceConnection . conf irmedRate  = 


createServiceConnection . requestedRate; 

conf irmServiceConnection . responseCode  = 


CONNECTION  REFUSED; 


txMsgHeader . cmdCode  =  CONFIRM_SERVICE_CONNECTION; 
txMsgHeader . dstSubsId  =  rxMsgHeader . srcSubsId; 
txMsgHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
txMsgHeader . dstCompId  =  rxMsgHeader . srcCompId; 
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convertConf irmServiceConnection (data, 
txMsgHeader . dataBytes) ; 

} 

break; 


txMsgHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
txMsgHeader . dataBytes  = 

&conf irmServiceConnection,  DATASIZE,  PACK) ; 
jmsSend (meedpdJms,  StxMsgHeader ,  data. 


case  CONFIRM_SERVICE_CONNECTION: 

convertConf irmServiceConnection (data, 

&conf irmServiceConnection,  DATASIZE,  UNPACK) ; 

//  USER:  Insert  code  here  to  handle  the  confirm  service 


connection  message 


break; 


case  ACT I VATE_SERVI CE_CONNECT I ON : 

convertActivateServiceConnection (data, 
SactivateServiceConnection,  DATASIZE,  UNPACK) ; 

//  USER:  Insert  code  here  to  handle  the  activate  service 


connection  message 


break; 


case  SUSPEND_SERVICE_CONNECTION: 

convert SuspendServiceConnect ion (data, 
SsuspendServiceConnection,  DATASIZE,  UNPACK) ; 

//  USER:  Insert  code  here  to  handle  the  suspend  service 


connection  message 


break; 


case  TERMINATE_SERVICE_CONNECTION : 

convertTerminateServiceConnection (data, 
SterminateServiceConnection,  DATASIZE,  UNPACK) ; 

if  ( terminateServiceConnection . cmdCode  == 


REPORT_JOINT_VELOCITY) 


meedpdScActive  =  FALSE; 

break; 


case  REQUEST_COMPONENT_CONTROL: 

convertRequestComponentControl (data, 
SrequestComponentControl,  DATASIZE,  UNPACK) ; 


if (meedpdUnderControl) 

{ 

if (requestComponentControl  > 
meedpdCtrlCmptAuthority)  //  Test  for  higher  authority 


RE JECT_COMPONENT_CONTROL ; 

meedpdCtrlCmptSubsId; 

meedpdCtrlCmptNodeld; 

meedpdCtrlCmptCompId; 

meedpdCtrlCmptlnstld; 

txMsgHeader . dataBytes) ; 

CONFI RM_COMPONENT_CONTROL ; 
rxMsgHeader . srcSubsId; 
rxMsgHeader . srcNodeld; 
rxMsgHeader . srcCompId; 


//  Terminate  control  of  current  component 
txMsgHeader . cmdCode  = 

txMsgHeader . dstSubsId  = 

txMsgHeader . dstNodeld  = 

txMsgHeader . dstCompId  = 

txMsgHeader . dstlnstld  = 

txMsgHeader . dataBytes  =  0; 

jmsSend (meedpdJms,  StxMsgHeader ,  data. 


//  Accept  control  of  new  component 
txMsgHeader . cmdCode  = 

txMsgHeader . dstSubsId  = 

txMsgHeader . dstNodeld  = 

txMsgHeader . dstCompId  = 
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rxMsgHeader . srclnstld; 

convertConf irmComponentControl (data, 
txMsgHeader . dataBytes)  ; 
rxMsgHeader . srclnstld; 
rxMsgHeader . srcCompId; 
rxMsgHeader . srcNodeld; 
rxMsgHeader . srcSubsId; 
requestComponentControl ; 


txMsgHeader . dstlnstld  = 

conf irmComponentControl . asByte  =  0; 
txMsgHeader . dataBytes  = 

&conf irmComponentControl,  DATASIZE,  PACK) ; 

jmsSend (meedpdJms,  StxMsgHeader ,  data, 

meedpdCtrlCmptlnstld  = 

meedpdCtrlCmptCompId  = 

meedpdCtrlCmptNodeld  = 

meedpdCtrlCmptSubsId  = 

meedpdCtrlCmptAuthority  = 


meedpdCtrlCmptSubsId  | | 
meedpdCtrlCmptCompId  | | 


else 

{ 

if  ( 

rxMsgHeader . srcNodeld  != 

rxMsgHeader . srclnstld  != 

{ 


RE JECT_COMPONENT_CONTROL; 
rxMsgHeader . srcSubsId; 
rxMsgHeader . srcNodeld; 
rxMsgHeader . srcCompId; 
rxMsgHeader . srclnstld; 


data,  txMsgHeader . dataBytes)  ; 


} 


rxMsgHeader . srcSubsId  != 
meedpdCtrlCmptNodeld  | | 

rxMsgHeader . srcCompId  != 
meedpdCtrlCmptlnstld  ) 

txMsgHeader . cmdCode  = 

txMsgHeader . dstSubsId  = 

txMsgHeader . dstNodeld  = 

txMsgHeader . dstCompId  = 

txMsgHeader . dstlnstld  = 

txMsgHeader . dataBytes  =  0; 
jmsSend (meedpdJms,  StxMsgHeader , 


control 


else  //  Not  currently  under  component  control,  so  give 


convertConf irmComponentControl (data, 
txMsgHeader . dataBytes)  ; 


txMsgHeader . cmdCode  =  CONFIRM_COMPONENT_CONTROL; 

txMsgHeader . dstSubsId  =  rxMsgHeader . srcSubsId; 
txMsgHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
txMsgHeader . dstCompId  =  rxMsgHeader . srcCompId; 
txMsgHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
conf irmComponentControl . asByte  =  0; 
txMsgHeader . dataBytes  = 

&conf irmComponentControl,  DATASIZE,  PACK) ; 
jmsSend (meedpdJms,  StxMsgHeader ,  data, 

meedpdCtrlCmptlnstld  =  rxMsgHeader . srclnstld; 
meedpdCtrlCmptCompId  =  rxMsgHeader . srcCompId; 
meedpdCtrlCmptNodeld  =  rxMsgHeader . srcNodeld; 
meedpdCtrlCmptSubsId  =  rxMsgHeader . srcSubsId; 
meedpdCtrlCmptAuthority  =  requestComponentControl; 
meedpdUnderControl  =  TRUE; 


break; 


case  RELEASE_COMPONENT_CONTROL : 

meedpdUnderControl  =  FALSE; 
break; 


case  CONFIRM_COMPONENT_CONTROL: 

convertConf irmComponentControl (data, 
&conf irmComponentControl,  DATASIZE,  UNPACK) ; 
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//  USER:  Insert  code  here  to  handle  the  confirm  component 

control  message  if  needed 

break; 


case 

control  message  if  needed 


RE JECT_COMPONENT_CONTROL : 

//  USER:  Insert  code  here  to  handle  the  reject  component 
break; 


case  QUERY_COMPONENT_AUTHORITY: 

txMsgHeader . cmdCode  =  RE  PORT_COMPONENT_AUTHORI T Y ; 

txMsgHeader . dstSubsId  =  rxMsgHeader . srcSubsId; 
txMsgHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
txMsgHeader . dstCompId  =  rxMsgHeader . srcCompId; 
txMsgHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
componentAuthority  =  meedpdAuthority; 

txMsgHeader . dataBytes  =  convertComponentAuthority (data, 
ScomponentAuthority,  DATASIZE,  PACK) ; 

jmsSend (meedpdJms,  StxMsgHeader ,  data. 


txMsgHeader . dataBytes)  ; 


break; 


case  QUERY_COMPONENT_STATUS : 

txMsgHeader . cmdCode  =  REPORT_COMPONENT_STATUS; 
txMsgHeader . dstSubsId  =  rxMsgHeader . srcSubsId; 
txMsgHeader . dstNodeld  =  rxMsgHeader . srcNodeld; 
txMsgHeader . dstCompId  =  rxMsgHeader . srcCompId; 
txMsgHeader . dstlnstld  =  rxMsgHeader . srclnstld; 
componentStatus . primaryStatus . asByte  =  meedpdState; 
txMsgHeader . dataBytes  =  convertComponentStatus (data, 
ScomponentStatus,  DATASIZE,  PACK) ; 

jmsSend (meedpdJms,  StxMsgHeader ,  data, 

txMsgHeader . dataBytes) ; 

break; 


DATASIZE,  UNPACK) ; 
authority  message  if 


case  REPORT_COMPONENT_AUTHORITY: 

convertComponentAuthority (data,  ScomponentAuthority, 

//  USER:  Insert  code  here  to  handle  the  report  component 

needed 

break; 


UNPACK) ; 


case  RE PORT_COMPONENT_S TATU S : 

convertComponentStatus (data. 


if (rxMsgHeader . srcCompId  == 
meedpdPmState  = 

componentStatus .primaryStatus . asField . primaryStatusCode; 

break; 


ScomponentStatus,  DATASIZE, 
PRIMITIVE_MANIPULATOR) 


case  SET_END_EFFECTOR_PATH_MOTION : 

convertEndEf fectorPathMotion (data, 
SsetEndEf fectorPathMotion,  DATASIZE,  UNPACK) ; 

break; 


UNPACK) ; 


case  REPORT_JOINT_EFFORT: 

convert JointEf fort (data,  SreportedJointEf fort ,  DATASIZE, 
break; 


case  RE PORT_MAN I PULATOR_S PEC I F I CAT I ON S : 

convertManipulatorSpecif ications (data, 
SreportedManipulatorSpecif ications,  DATASIZE,  UNPACK) ; 

break; 


DATASIZE,  UNPACK) ; 


case  REPORT_JOINT_POSITION : 

convert JointPosition (data,  &reportedJointPosition, 

break; 


case  QUERY_TOOL_POINT: 
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txMsgHeader . cmdCode  = 
txMsgHeader . dstlnstld 
txMsgHeader . dstCompId 
txMsgHeader . dstNodeld 
txMsgHeader . dstSubsId 
txMsgHeader . dataBytes 
&reportedTool Point ,  DATASIZE,  PACK) ; 

jmsSend (meedpdJms, 

txMsgHeader .dataBytes)  ; 

break; 


RE  PORT_TOOL_PO I  NT  ; 

=  rxMsgHeader . srclnstld; 
=  rxMsgHeader . srcCompId; 
=  rxMsgHeader . srcNodeld; 
=  rxMsgHeader . srcSubsId; 
=  convertToolPoint (data, 

& txMsgHeader ,  data. 


} 


default : 

break; 


meedpdNodemgrThreadRunning  =  FALSE; 
pthread_exit (NULL) ; 


//////////////////////////////////////////////////////////////////////////////////// 
//  Function  used  to  send  a  message  to  the  primitive  manipulator  and  query  for 
//  component  status.  Only  the  primitive  manipulator  component  performs  the  initial 
//  system  checks.  Thus  it  is  essential  that  primitive  manipulator  is  running  and  its 
//  status  is  ready  before  any  other  components  are  started  up. 
//////////////////////////////////////////////////////////////////////////////////// 
void  meedpdQueryPmComponentStatus (void) 

{ 

jrrih  _t  txMsg; 

unsigned  char  data [DATASIZE] ; 
double  requestTimeOutSec  =  0.1; 
static  double  lastRequestTimeSec  =  0; 

//  Setup  Transmit  msg 

txMsg . isExpMsg  =  0; 

txMsg . priority  =  6; 

txMsg. acknCtrl  =  0; 

txMsg . isSvcMsg  =  0; 

txMsg . version  =  1; 

txMsg . reserved  =  0; 

txMsg . srclnstld  =  meedpdlnstld; 

txMsg. srcCompId  =  MANIPULATOR_END_EFFECTOR_DISCRETE_POSE_DRIVER; 

txMsg . srcNodeld  =  meedpdNodeld; 

txMsg. srcSubsId  =  meedpdSubsId; 

txMsg. dataCtrl  =  0; 

txMsg . seqNumber  =  0; 

txMsg. cmdCode  =  QUERY_COMPONENT_STATUS; 
txMsg . dstlnstld  =  1; 

txMsg. dstCompId  =  PRIMITIVE_MANIPULATOR; 
txMsg . dstNodeld  =  meedpdNodeld; 
txMsg. dstSubsId  =  meedpdSubsId  ; 
txMsg. dataBytes  =  0; 

if (  (getTimeSeconds ( )  -  lastRequestTimeSec)  >=  requestTimeOutSec) 

{ 

jmsSend (meedpdJms,  &txMsg,  data,  txMsg. dataBytes ) ; 
lastRequestTimeSec  =  getTimeSeconds (); 


//////////////////////////////////////////////////////////////////////////////////// 
//  Function  used  to  set  up  a  message  sent  to  manipulator  joint  position  sensor 
//  to  obtain  current  joint  position  data. 

//////////////////////////////////////////////////////////////////////////////////// 

void  meedpdQueryMjps JointPosition (void) 

{ 

j rnh  _t  txMsg; 

unsigned  char  data [DATASIZE] ; 
double  requestTimeOutSec  =  0.1; 
static  double  lastRequestTimeSec  =  0; 


174 


//  Setup  Transmit  msg 

txMsg . isExpMsg  =  0; 

txMsg . priority  =  6; 

txMsg. acknCtrl  =  0; 

txMsg . isSvcMsg  =  0; 

txMsg . version  =  1; 

txMsg . reserved  =  0; 

txMsg. srcSubsId  =  meedpdSubsId; 

txMsg . srcNodeld  =  meedpdNodeld; 

txMsg. srcCompId  =  MANIPULATOR_END_EFFECTOR_DISCRETE_POSE_DRIVER; 
txMsg . srclnstld  =  meedpdlnstld; 
txMsg. dataCtrl  =  0; 
txMsg . seqNumber  =  0; 

txMsg. cmdCode  =  QUERY_JOINT_POSITION; 
txMsg . dstlnstld  =  1; 

txMsg. dstcompld  =  MANIPULATOR_JOINT_POSITION_SENSOR; 
txMsg . dstNodeld  =  meedpdNodeld; 
txMsg. dstSubsId  =  meedpdSubsId  ; 
txMsg. dataBytes  =  0; 

if (  (getTimeSeconds ( )  -  lastRequestTimeSec)  >=  requestTimeOutSec) 

{ 

jmsSend (meedpdJms,  &txMsg,  data,  txMsg. dataBytes ) ; 
lastRequestTimeSec  =  getTimeSeconds () ; 


//////////////////////////////////////////////////////////////////////////////////// 
//  Function  used  to  set  up  a  message  sent  to  the  primitive  manipulator  to  obtain 
//  current  effort  values. 

//////////////////////////////////////////////////////////////////////////////////// 

void  meedpdQueryPmJointEf f ort (void) 

{ 

j mh  _t  txMsg; 

unsigned  char  data [DATASIZE] ; 
double  requestTimeOutSec  =  0.1; 
static  double  lastRequestTimeSec  =  0; 

//  Setup  Transmit  msg 

txMsg . isExpMsg  =  0; 

txMsg . priority  =  6; 

txMsg. acknCtrl  =  0; 

txMsg . isSvcMsg  =  0; 

txMsg . version  =  1; 

txMsg . reserved  =  0; 

txMsg. srcSubsId  =  meedpdSubsId; 

txMsg . srcNodeld  =  meedpdNodeld; 

txMsg. srcCompId  =  MANIPULATOR_END_EFFECTOR_DISCRETE_POSE_DRIVER; 

txMsg . srclnstld  =  meedpdlnstld; 

txMsg. dataCtrl  =  0; 

txMsg . seqNumber  =  0; 

txMsg. cmdCode  =  QUERY_ JO I NT_E  FFORT ; 

txMsg . dstlnstld  =  1; 

txMsg. dstcompld  =  PRIMITIVE_MANIPULATOR; 
txMsg . dstNodeld  =  meedpdNodeld; 
txMsg. dstSubsId  =  meedpdSubsId  ; 
txMsg. dataBytes  =  0; 

if (  (getTimeSeconds ( )  -  lastRequestTimeSec)  >=  requestTimeOutSec) 

{ 

jmsSend (meedpdJms,  &txMsg,  data,  txMsg. dataBytes ) ; 
lastRequestTimeSec  =  getTimeSeconds (); 


//////////////////////////////////////////////////////////////////////////////////// 
//  Function  used  to  set  up  a  message  sent  to  the  primitive  manipulator  to  obtain 
//  manipulator  specifications. 

//////////////////////////////////////////////////////////////////////////////////// 

void  meedpdQueryPmManipulatorSpecif ications (void) 

{ 
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jmh_t  txMsg; 

unsigned  char  data [DATASIZE] ; 
double  requestTimeOutSec  =  0.1; 
static  double  lastRequestTimeSec  =  0; 

//  Setup  Transmit  msg 

txMsg . isExpMsg  =  0; 

txMsg . priority  =  6; 

txMsg. acknCtrl  =  0; 

txMsg . isSvcMsg  =  0; 

txMsg . version  =  1; 

txMsg . reserved  =  0; 

txMsg. srcSubsId  =  meedpdSubsId; 

txMsg . srcNodeld  =  meedpdNodeld; 

txMsg. srcCompId  =  MANIPULATOR_END_EFFECTOR_DISCRETE_POSE_DRIVER; 
txMsg . srclnstld  =  meedpdlnstld; 
txMsg. dataCtrl  =  0; 
txMsg . seqNumber  =  0; 

txMsg. cmdCode  =  QUERY_MANIPULATOR_SPECIFICATIONS; 
txMsg . dstlnstld  =  1; 

txMsg. dstcompld  =  PRIMITIVEJXLANIPULATOR; 
txMsg . dstNodeld  =  meedpdNodeld; 
txMsg. dstSubsId  =  meedpdSubsId  ; 
txMsg. dataBytes  =  0; 

if (  (getTimeSeconds ( )  -  lastRequestTimeSec)  >=  requestTimeOutSec) 

{ 

jmsSend (meedpdJms,  &txMsg,  data,  txMsg. dataBytes ) ; 
lastRequestTimeSec  =  getTimeSeconds () ; 

} 


//////////////////////////////////////////////////////////////////////////////////// 
//  Function  used  to  set  up  a  message  sent  to  the  primitive  manipulator  with  new 
//  joint  effort  values. 

//////////////////////////////////////////////////////////////////////////////////// 

void  meedpdSetPmJointEf f ort (void) 

{ 

jrrih  _t  txMsg; 

double  requestTimeOutSec  =  0.1; 
static  double  lastRequestTimeSec  =  0; 
unsigned  char  data [DATASIZE] ; 

//Setup  Transmit  Message 

txMsg . isExpMsg  =  0; 

txMsg . priority  =  6; 

txMsg. acknCtrl  =  0; 

txMsg . isSvcMsg  =  0; 

txMsg . version  =  1; 

txMsg . reserved  =  0; 

txMsg . srclnstld  =  meedpdlnstld; 

txMsg. srcCompId  =  MANIPULATOR_END_EFFECTOR_DISCRETE_POSE_DRIVER; 

txMsg . srcNodeld  =  meedpdNodeld; 

txMsg. srcSubsId  =  meedpdSubsId; 

txMsg. dataCtrl  =  0; 

txMsg . seqNumber  =  0; 

txMsg. cmdCode  =  SET_JOINT_EFFORT; 

txMsg . dstlnstld  =  1; 

txMsg. dstcompld  =  PRIMITIVE_MANIPULATOR; 
txMsg . dstNodeld  =  meedpdNodeld; 
txMsg. dstSubsId  =  meedpdSubsId; 

txMsg . dataBytes  =  convert JointEf fort (data,  &set JointEf fort,  DATASIZE,  PACK) ; 

if (  (getTimeSeconds ( )  -  lastRequestTimeSec)  >=  requestTimeOutSec) 

{ 

jmsSend (meedpdJms,  &txMsg,  data,  txMsg. dataBytes ) ; 
lastRequestTimeSec  =  getTimeSeconds () ; 

} 


//////////////////////////////////////////////////////////////////////////////////// 
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//  Function  used  to  perform  closed  loop  end-effector  position  control 

/  /  /  /  /  /  /  /  /  /  III  /  /  /  /  /  /  /  /  / /// III  III  /  /  / /// / / / III  III  III  /  /  /  /  /  III  /  /  /  III  III  III  III  III  III  III  /  /  / 

void  endEf f ectorDiscretePoseDriver (double  *cmd_position,  double  *curr_position,  double 
*cmd_ef fort) 

{ 

int  i; 

double  k [NUM_JOINTS] ; 

for  (1=0;  i<NUM_JOINTS;  i++) 

{ 

if  (i  <  3)  k [ i ]  =34; 
else  if  (i  ==  3)  k[i]  =  34; 
else  if  (i  ==  4)  k[i]  =  15; 
else  k [ i ]  =  14 ; 


if  (i 

.  ==  0) 

cmd  effort | 

;i]  = 

(  (cmd 

position | 

(ij 

-  curr 

position | 

;i] ) 

* 

k  1 

(i]  ) 

* 

J1 

RAD_ 

TO_ 

ENC 

*  100 

/  112000; 

else 

if  (i  ==  1) 

cmd  effort | 

(i]  = 

(  (cmd 

position | 

(i] 

-  curr 

position | 

(i]  ) 

* 

k  I 

:i] ) 

* 

J2 

_RAD_ 

_T0_ 

ENC 

*  100 

/  112000; 

else 

if  (i  ==  2) 

cmd  effort | 

;i|  = 

(  (cmd 

position | 

:i] 

-  curr 

position | 

:i] ) 

* 

k  I 

:i] ) 

* 

J3_ 

_RAD_ 

_T0_ 

ENC 

*  100 

/  112000; 

else 

if  (i  ==  3) 

cmd  effort | 

;i]  = 

(  (cmd 

position | 

:i] 

-  curr 

position | 

;i] ) 

* 

k  1 

;i] ) 

* 

J4 

RAD_ 

TO_ 

ENC 

*  100 

/  35000; 

else 

if  (i  ==  4) 

cmd  effort | 

:i]  = 

(  (cmd 

position | 

:i] 

-  curr 

position | 

:i] ) 

* 

k  | 

:i] ) 

* 

J5_ 

_RAD_ 

_T°_ 

ENC 

*  100 

/  16000; 

else 

cmd  effort | 

:i]  = 

(  (cmd 

position | 

;ij 

-  curr 

position | 

:i] ) 

* 

k  | 

:i] ) 

* 

J6 

RAD 

TO 

ENC 

*  100 

/  15500; 

} 

} 

void  PmatrixToQuaternion (double  *orientA,  double  *orientS,  double  *curr_orientation) 

{ 

double  T,  S; 
double  W,  X,  Y,  Z; 
double  MO,  Ml,  M2; 
double  M4,  M5,  M6; 
double  M8,  M9,  M10; 

MO  =  orientA[0]; 

M4  =  orientA[l]; 

M8  =  orientA[2]; 

Ml  =  orients [ 1 ] *orientA [ 2 ]  -  orients [2 ] *orientA [ 1 ] ; 

M5  =  orients [2 ] *orientA [ 0 ]  -  orients [ 0 ] *orientA [2 ] ; 

M9  =  orients [ 0 ] *orientA [ 1 ]  -  orients [ 1 ] *orientA [ 0 ] ; 

M2  =  orients [0] ; 

M6  =  orients [ 1 ] ; 

M10  =  orientS[2]; 

T  =  MO  +  M5  +  M10  +  1; 

S  =  0.5/sqrt (T) ; 

W  =  0.25/S; 

X  =  (M9  -  M6) *S; 

Y  =  (M2  -  M8) *S; 

Z  =  (M4-M1)*S; 

curr_orientation [ 0 ]  =  W; 
curr_orientation [ 1 ]  =  X; 
curr_orientation [ 2 ]  =  Y; 
cur reorientation [3]  =  Z; 

} 
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void  PquaternionToMatrix (double  *cmd_orientation, double  *des_orientS ,  double 
*des_orientA) 

{ 


double 

W, 

X,  Y, 

.  Z; 

double 

MO, 

.  Ml, 

M2; 

double 

M4 , 

.  M5, 

M6  ; 

double 

M8 , 

.  M9 , 

M10 

W  =  cmd_orientation [ 0 ] ; 
X  =  cmd_orientation  [  1 ] ; 
Y  =  cmd_orientation  [  2 ] ; 
Z  =  cmd_orientation [ 3 ] ; 


MO  ^ 

=  1  -  2*Y 

*  Y 

- 

2*Z* 

M4  ^ 

=  2*X*Y  + 

2 

*z^ 

*W; 

M8  : 

=  2*X*Z  - 

2 

*Y- 

*W; 

Ml  ^ 

=  2*X*Y  - 

2 

*Z^ 

*W; 

M5  : 

=  1  -  2*X 

*X 

- 

2*Z* 

M9  ^ 

=  2*Y*Z  + 

2 

*X^ 

*W; 

M2  ^ 

=  2*X*Z  + 

2 

*Y- 

*W; 

M6  : 

=  2*Y*Z  - 

2 

*W; 

M10 

=  1-2* 

X* 

X  - 

-  2  *  Y 

des 

orientA [ 

0] 

= 

MO; 

des 

orientA [ 

1] 

= 

M4  ; 

des 

orientA [ 

2] 

= 

M8  ; 

des 

orients [ 

0] 

= 

M2; 

des 

orients [ 

1] 

= 

M6  ; 

des 

orients [ 

2] 

= 

M10; 

///////////////////////////////////////////////////////////////////////////////////////// 

//  File:  meedpd.h 

//  Version:  0.1  Original  Creation 

//  Written  by:  Ognjen  Sosa  (ognjensosa@hotmail.com) 

Template  by:  Tom  Galluzzo  (galluzzt@ufl.edu) 

Date:  09/28/2004 


// 

// 

// 

//  Description: 
// 


This  file  contains  the  skeleton  C  header  file  code  for  implementing 
the  meedpd.c  file 

///////////////////////////////////////////////////////////////////////////////////////// 


#ifndef  MEEDPD_H 
#def ine  MEEDPD_H 

#include  "jaus.h" 

#ifndef  FALSE 
#def ine  FALSE  0 
#endif 

#ifndef  TRUE 
#define  TRUE  1 
#endif 


#def ine  MEEDPD_NODE_MANAGER_CHECKIN_ERROR  -1 

#def ine  MEEDPD_JMS_OPEN_ERROR  -2 

#def ine  MEEDPD_STARTUP_BEFORE_SHUTDOWN_ERROR  -3 

#def ine  MEEDPD_STATE_THREAD_CREATE_ERROR  -4 

#def ine  MEEDPD_NODEMGR_THREAD_CREATE_ERROR  -5 

#def ine  MEEDPD  THREAD  TIMEOUT  SEC  1.0 


//  Public 

int  meedpdStartup (void) ; 
int  meedpdShutdown (void) ; 
int  meedpdGetState (void)  ; 
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unsigned  char  meedpdGetlnstanceld (void) ; 
unsigned  char  meedpdGetCompId (void) ; 
unsigned  char  meedpdGetNodeld (void) ; 
unsigned  char  meedpdGetSubsystemld (void) ; 
double  meedpdGetUpdateRate (void) ; 
int  meedpdGetPmState (void) ; 

//  Query  messages  requested  by  this  component 

void  meedpdQueryPmComponentStatus (void) ; 

void  meedpdQueryPmJointEf fort (void) ; 

void  meedpdQueryMjps JointPosition (void) ; 

void  meedpdQueryPmManipulatorSpecif ications (void) ; 

//  Set  messages  that  this  component  commands 
void  meedpdSetPmJointEf fort (void) ; 

//  Accessors  to  incoming  information 
toolPoint_t  *  meedpdSetToolPoint (void) ; 

endEf fectorPathMotion_t  *  meedpdSetEndEf fectorPathMotion (void) ; 
jointEf fort_t  *  meedpdReport JointEf fort (void) ; 

manipulatorSpecif ications_t  *  meedpdReportManipulatorSpecif ications (void) ; 
jointPosition_t  *  meedpdReport JointPosition (void) ; 

//  Accessors  to  outgoing  information 
toolPoint_t  *  meedpdReportToolPoint (void) ; 
jointEf fort_t  *  meedpdSet JointEf fort (void)  ; 

double  *  meedpdGetCurrentOrientation (void) ; 
int  meedpdGetPose (void)  ; 
double  meedpdGetPoseTime (void) ; 
double  meedpdGetCurrentTime (void) ; 

double  *  meedpdGetCurrentEndEf fectorPosition (void) ; 
double  *  meedpdGetCurrentEndEf fectorOrientation (void) ; 
void  PmatrixToQuaternion (double  * ,  double  *,  double  *) ; 
void  PquaternionToMatrix (double  * ,  double  *,  double  *) ; 

void  endEf fectorDiscretePoseDriver (double  *,  double  *,  double  *) ; 

#endif  //  MEEDPD  H 


B.6  The  Mc.c  File  and  the  Corresponding  Header  Mc.h 

///////////////////////////////////////////////////////////////////////////////////////// 

//  File:  mc.c 

//  Version:  0.1  Original  Creation 

//  Written  by:  Ognjen  Sosa  (ognjensosa@hotmail.com) 

//  Date:  09/28/2004 

// 

//  Description:  This  file  contains  node  startup  and  shutdown  functions.  "Manipulator 
//  Control"  (me)  node  is  used  to  spawn  all  the  neccessary  threads.  Due  to 

//  very  low  bandwith  to  the  Galil  controller,  only  one  of  the  driver 

//  components  can  be  running  at  the  time. 

///////////////////////////////////////////////////////////////////////////////////////// 

#include  <stdio.h> 

#include  <time.h> 

#include  <unistd.h> 

#include  "mc.h" 

int  roboWorksReady  =  0; 

int  mcStartup (void) 

{ 

char  choice; 
int  i; 

//  Check  if  RoboWorks  software  is  being  used 
do  { 
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printf("Are  you  using  RoboWorks  3D  visualization  tool?  (y  or  n) :  "); 
scant ( "%c" , &choice) ; 

}  while  (choice  !=  ' y'  &&  choice  !=  'n'); 

if  (choice  ==  ' y ' ) 

roboWorksReady  =  1; 
else  if  (choice  ==  ' n') 

roboWorksReady  =  0; 

//  Start  controllers 
startGALILController ( ) ; 
if  (getGalilReady ( )  ==  0) 

{ 

printf ( "Could  not  establish  communications  with  GALIL  controller . \n" ) ; 
while  (getGalilReady ( )  ==  0) 

{ 

startGALILController ( ) ; 
usleep (10000) ; 

} 

printf ( "Communications  with  GALIL  controller  established . \n" ) ; 

} 

else 

printf ( "Communications  with  GALIL  controller  established . \n" ) ; 

//  Reset  Galil 
i  =  resetGalil ( ) ; 
if  (i  ==  0) 

printf ( "GALIL  reset  successful . \n" ) ; 

else 

printf ( "GALIL  reset  failed. \n"); 

startVALController  ( )  ; 
if  (getValReady ( )  ==  1) 

{ 

printf ("VAL  controller  is  not  on.  Switch  main  power  ON.\n"); 
while  (getValReady ( )  ==  1) 

{ 

startVALController  ( ) ; 
usleep (10000) ; 

} 

printf ("VAL  controller  is  on.\n") ; 

} 

else 

printf ("VAL  controller  is  on.\n") ; 

//  Allows  for  remote  operation  of  the  manipulator 
startARM ( ) ; 

if  (getArmReady ( )  ==  1) 

{ 

printf ( "Switch  PUMA  762  on.\n") ; 
while  (getArmReady ( )  ==  1) 

( 

StartARM ( ) ; 
usleep  (1000) ; 

} 

printf ("PUMA  762  is  on . \n" ) ; 

) 

else 

printf ("PUMA  762  is  on . \n" ) ; 


//  Start  up  the  interface  thread 
interf aceStartup ( ) ; 

//  Start  component  threads 

//  Primitive  manipulator  and  sensor  components  should  always  be  running 
//  Comment  out  startup  routines  of  driver  components  you  are  not  planning  to  use 
if (pmStartup ( ) ) 

{ 

cError ("me:  pmStartup  failed\n"); 
mcShutdown ( ) ; 
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return  MC  PM  STARTUP  FAILED; 


if  (mjpsStartup ( ) ) 

{ 

cError ("me:  mjpsStartup  failed\n" 
mjpsShutdown ( ) ; 

return  MC_MJPS_STARTUP_FAILED; 

} 

if  (mj vsStartup ( ) ) 

{ 

cError ("me:  mjvsStartup  failed\n" 
mj vs Shutdown ( ) ; 

return  MC_MJVS_STARTUP_FAILED; 

} 

if  (mj ftsStartup ( ) ) 

{ 

cError ("me:  mj ftsStartup  failed\n 
mj  f tsShutdown ( ) ; 

return  MC_MJFTS_STARTUP_FAILED; 

} 

if  (mjpdStartup ( ) ) 

{ 

cError ("me:  mjpdStartup  failed\n" 
mjpdShutdown ( ) ; 

return  MC_MJPD_STARTUP_FAILED; 

} 

if  (mj vdStartup ( ) ) 

{ 

cError ("me:  mjvdStartup  failed\n" 
mj vdShutdown ( ) ; 

return  MC_MJVD_STARTUP_FAILED; 

} 


if  (mjmdStartup ( ) ) 

{ 

cError ("me:  mjmdStartup  failed\n" 
mjmdShutdown ( ) ; 

return  MC_MJMD_STARTUP_FAILED; 

} 

if  (meepdStartup ( ) ) 

{ 

cError ("me:  meepdStartup  failed\n 
meepdShutdown ( ) ; 

return  MC_MEEPD_STARTUP_FAILED; 

} 

if  (meedpdStartup ( ) ) 

{ 

cError ("me:  meedpdStartup  failedV 
meedpdShutdown ( ) ; 

return  MC_MEEDPD_STARTUP_FAILED; 

} 


return  0; 
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int  mcShutdown (void) 

{ 

pmShutdown ( ) ; 
mjpsShutdown ( ) ; 
mj vs Shutdown ( ) ; 
mj  f tsShutdown ( ) ; 
mjpdShutdown ( ) ; 
mj vdShutdown ( ) ; 
mjmdShutdown ( ) ; 
meepdShutdown ( ) ; 
meedpdShutdown ( ) ; 
interf aceShutdown ( ) ; 

return  0; 

} 

int  getRoboWorksReady ( void)  {  return  roboWorksReady ;  } 

const  char  *mcGetName (void) 

{ 

static  const  char  *mcName  =  MC_NAME_STRING; 
return  mcName; 

} 


///////////////////////////////////////////////////////////////////////////////////////// 

//  File:  mc.h 

//  Version:  0.1  Original  Creation 

//  Written  by:  Ognjen  Sosa  (ognjensosa@hotmail.com) 

//  Date:  09/28/2004 

// 

//  Description:  This  file  contians  function  prototypes  required  for  mc.c 

///////////////////////////////////////////////////////////////////////////////////////// 

#ifndef  N0DE_H 
#define  N0DE_H 

#ifndef  TRUE 
#define  TRUE  1 
#endif 

#ifndef  FALSE 
#def ine  FALSE  0 
#endif 

#define  MC_NAME_STRING  "Manipulator  Control" 

#def ine  MC_PM_STARTUP_FAILED  -1 
#def ine  MC_MJPS_STARTUP_FAILED  -2 

#def ine  MC_MJVS_STARTUP_FAILED  -3 
#def ine  MC_MJFTS_STARTUP_FAILED  -4 

#def ine  MC_MJPD_STARTUP_FAILED  -5 

#def ine  MC_MEEPD_STARTUP_FAILED  -6 
#def ine  MC_MJVD_STARTUP_FAILED  -7 
#def ine  MC_MJMD_STARTUP_FAILED  -9 
#def ine  MC_MEEDPD_STARTUP_FAILED  -10 

int  mcStartup (void) ; 
int  mcShutdown (void)  ; 

int  getRoboWorksReady (void)  ; 

const  char  *mcGetName (void) ; 

#include  "pm.h" 

#include  "mjps.h" 

#include  "mjvs.h" 

#include  "mjfts.h" 

#include  "mjpd.h" 

#include  "mjvd.h" 


182 


#include  "mjmd.h" 

#include  "meepd.h" 

#include  "meedpd.h" 

#include  "galillnterface . h" 
#include  "mcConstants . h" 
#include  "cpplnterface . h" 

#include  "logLib.h" 

#include  "timeLib.h" 

#include  "dmclnx.h" 

#include  "RoboTalk.h" 

#endif  //  MC  H 


B.7  The  Main.c  File 

///////////////////////////////////////////////////////////////////////////////////////// 

//  File:  main.c 

//  Version:  0.1  Original  Creation 

//  Written  by:  Ognjen  Sosa  (ognjensosa@hotmail.com) 

//  Template  by:  Tom  Galluzzo  (galluzzt@ufl.edu) 

//  Date:  09/28/2004 

// 

//  Description:  This  file  contains  the  C  code  used  to  initiate  node  startup  and  display 
//  component  information. 

///////////////////////////////////////////////////////////////////////////////////////// 

#include  <curses.h> 

#include  <ctype.h> 

#include  <errno.h> 

#include  <signal.h> 

#include  <stdio.h> 

#include  <stdlib.h> 

#include  <string.h> 

#include  <sys/stat.h> 

#include  <sys/types ,h> 

#include  <termios.h> 

#include  <time.h> 

#include  <unistd.h> 

#include  "pm.h" 

#include  "galillnterface. h" 

#include  "cpplnterface. h" 

#include  "logLib.h" 

#include  "mc.h" 

#include  "timeLib.h" 

#include  "mcConstants .h" 

#include  "mjvs.h" 

#include  "mjps.h" 

#include  "mjfts.h" 

#include  "mjpd.h" 

#include  "mjvd.h" 

#include  "mjmd.h" 

#include  "meepd.h" 

#ifndef  TRUE 
#define  TRUE  1 
#endif 

#ifndef  FALSE 
#def ine  FALSE  0 
#endif 

void  updateScreen (int  page); 

void  signallnterventionHandler (int) ; 

const  char  * jausStateString (int) ; 


int  mainRunning; 


183 


int  main(int  argCount,  char  **argString) 

{ 

int  i; 

char  choice=0; 
int  verbose  =  FALSE; 
int  debugLevel  =  0; 
struct  termios  newTermios; 
struct  termios  stored; 

FILE  *logFile  =  NULL; 
char  logFileStr [128]  =  { 0 } ; 
time_t  timeStamp; 
char  timeString [ 64 ] ; 
int  page  =  0; 

//Get  and  Format  Time  String 
time ( StimeStamp) ; 

strftime  (timeString,  63,  "%m-%d-%Y  %X",  localtime ( StimeStamp) ) ; 
system ( "clear" ) ; 

printf ( "main ;  CIMAR  Core  Executable:  %s\n",  timeString); 

if (mkdir ("/var/log/CIMAR/",  0777)  <  0) 

{ 

if (errno  ! =  EEXIST) 

{ 

perror ( "main :  Error") ; 
return  -1; 

} 

} 

for(i=l;  i<argCount;  i++) 

{ 

if (argString [i] [0]  ==  '-') 

{ 

switch (argString [i] [1]) 

{ 

case  ' v ' : 

verbose  =  TRUE; 
setLogVerbose (TRUE) ; 
break; 


case  ' d ' : 

if (argString [i] [2 ]  ==  '+') 

{ 

setDebugLogic (DEBUG_GREATER_THAN) ; 
debugLevel  =  atoi ( &argString [i] [3] ) ; 

} 

else  if (argString [i] [2 ]  ==  '-') 

{ 

setDebugLogic (DEBUG_LESS_THAN) ; 
debugLevel  =  atoi ( &argString [i]  [3] )  ; 

} 

else  if (argString [i] [2 ]  ==  '=') 

{ 

setDebugLogic (DEBUG_EQUAL_TO) ; 
debugLevel  =  atoi ( &argString [i]  [3]  )  ; 

} 

else  if (argString [i] [2 ]  >=  'O'  &&  argString [ i ][ 2 ]  <= 


debugLevel  =  atoi ( &argString [i] [2 ] ) ; 

} 

else 

{ 

printf ( "main :  Incorrect  use  of 


arguments\n" )  ; 


break; 
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printf ( "main :  Entering  debug  level:  %d\n", 

debugLevel) ; 

setDebugLevel (debugLevel)  ; 
break; 


case 


%s\n",  argString [0]  ,  timestring); 


switching  to  default\n"); 


if (argCount  >  i+1  &&  argString [ i+1 ][ 0 ]  !=  '-') 

{ 

logFile  =  f open (argString [ i+1 ] ,  "w"); 
if(logFile  !=  NULL) 

{ 

fprintf (logFile,  "CIMAR  %s  Log  -- 
setLogFile (logFile)  ; 

} 

else  printf ( "main :  Error  creating  log  file, 


arguments\n" )  ; 


else 


printf ( "main :  Incorrect  use  of 


break; 
default : 

printf ( "main :  Incorrect  use  of  arguments\n" ) ; 
break; 


} 

if (logFile  ==  NULL) 

{ 

i  =  strlen (argString [0] )  -  1; 

while (i>0  &&  argString[0]  [ i  —  1 ]  !=  '/')  i--; 

sprintf (logFileStr,  " /var /log/CIMAR/%s . log" ,  SargString [ 0 ] [ i ] ) ; 
printf ( "main :  Creating  log:  %s\n",  logFileStr); 
logFile  =  fopen (logFileStr,  "w"); 
if (logFile  !=  NULL) 

{ 

fprintf (logFile,  "CIMAR  %s  Log  --  %s\n",  argString [0] ,  timestring) 
setLogFile (logFile) ; 

} 

else  printf ( "main :  ERROR:  Could  not  create  log  file\n"); 

//  BUG:  Else  pError  here 

} 


signal (SIGINT,  signallnterventionHandler ) ; 

cDebug(l,  "main:  Starting  Up  %s  Node  Software\n",  mcGetName ( ) ) ; 
if (mcStartup ( ) ) 

{ 

cError ("main:  %s  Node  Startup  failed\n",  mcGetName ()) ; 
cDebug(l,  "main:  Exiting  %s  Node  Software\n",  mcGetName ()) ; 
return  0; 


if (verbose) 

{ 

tcgetattr (0, &stored) ; 

memcpy ( SnewTermios , & stored, sizeof ( struct  termios ) ) ; 

//  Disable  canonical  mode,  and  set  buffer  size  to  0  byte(s) 

newTermios . c_lf lag  &=  (~ICAN0N) ; 

newTermios . c_lf lag  &=  (~ECHO) ; 

newTermios . c_cc [VTIME]  =  0; 

newTermios . c_cc [VMIN]  =  0; 

tcsetattr ( 0 , TCSANOW, SnewTermios ) ; 


else 
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//  Start  up  Curses  window 
initscr ( ) ; 
cbreak ( )  ; 
noecho  ( )  ; 

nodelay (stdscr,  1);  //  Don't  wait  at  the  getch ( )  function  if  the  user 

hasn't  hit  a  key 

} 

mainRunning  =  TRUE; 

while (mainRunning) 

{ 

if (verbose) 

{ 

choice  =  getc(stdin); 
switch (choice) 

i 

case  27:  //  Escape  Key  Pressed 
mainRunning  =  FALSE; 
break; 
default : 

usleep (1000) ; 
break; 


'O'; 


else 


choice  =  getch () ;  //  Get  the  key  that  the  user  has  selected 
switch (choice) 

{ 

case  27 : 

mainRunning  =  FALSE; 
break; 
default : 

usleep (1000) ; 

if (choice  >=  'O'  &&  choice  <=  '9')  page  =  choice  - 
break; 

} 

updateScreen (page)  ; 


usleep (25000) ; 

} 

if (verbose) 

tcsetattr (0, TCSANOW, ^stored) ; 

else 


//  Stop  Curses 
clear ( ) ; 
endwin ( ) ; 


cDebug(l,  "main:  Shutting  Down  %s  Node  Software\n",  mcGetName ( ) ) ; 
mcShutdown ( ) ; 

if(logFile  !=  NULL) 

{ 

fclose (logFile) ; 

} 

return  0; 


const  char  * jausStateString (int  jausState) 

{ 

static  const  char  * jausStateString [7 ]  =  {  "Initialize", 

"Ready" , 


Standby 
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"Shutdown" , 

"Failure" , 

"Emergency" , 

"Unknown" 

} ; 


if(jausState  <  0  ||  jausState  >  5)  return  jausStateString [6] ; 
else  return  jausStateString [jausState]  ; 


void  updateScreen (int  page) 

{ 

int  row  =  0,  column  =  0; 
int  pose; 

int  numSoln,  optSoln; 

double  totalCost; 

jointEf fort_t  ^effort; 

j ointPosition_t  ^position; 

jointVelocity_t  ^velocity; 

jointForceTorque_t  *forcetorque; 

manipulator Specifications^  * specs; 

j ointMotion_t  *motion; 

toolPoint_t  *toolpoint; 

endEf fectorPose_t  *endef fectorpose; 

endEf fectorPathMotion_t  *endef fectorpathmotion; 

double  *posePosition,  *maxVelocity,  *maxAcceleration,  *maxDeceleration; 

double  currentTime,  poseTime; 

double  *currentOrientation; 

double  * cur rentEndEf fector Posit ion; 

double  * cur rentEndEf fectorOrientation; 

clear ( ) ; 

switch (page) 

{ 

case  0 : 

mvprintw (row++, column, "CIMAR  %s  Node  Main  Screen",  mcGetNameO  ); 
row++; 

mvprintw (row++, column, "Low  Level  Manipulator  Control  Components:"); 
row++; 

mvprintw (row++, column, "  1)  Primitive  Manipulator"); 
row++; 

mvprintw (row++, column, "Manipulator  Sensor  Components:"); 
row++; 

mvprintw (row++, column, "  2)  Manipulator  Joint  Position  and  Velocity 

Sensors" ) ; 

mvprintw (row++, column, "  3)  Manipulator  Joint  Force/Torque  Sensor"); 
row++; 

mvprintw (row++, column, "Low  Level  Position  and  Velocity  Driver 

Components :"); 

row++; 

mvprintw (row++, column, "  4)  Manipulator  Joint  Positions  Driver"); 
mvprintw (row++, column, "  5)  Manipulator  End-Effector  Pose  Driver"); 
mvprintw (row++, column, "  6)  Manipulator  Joint  Velocities  Driver"); 
mvprintw (row++, column, "  7)  Manipulator  End_Ef fector  Velocity  State 

Driver" ) ; 

row++; 

mvprintw (row++, column, "Mid  Level  Position  and  Velocity  Driver 

Components :"); 

row++; 

mvprintw (row++, column, "  8)  Manipulator  Joint  Move  Driver"); 
mvprintw (row++, column, "  9)  Manipulator  End-Effector  Discrete  Pose 

Driver  Component") ; 

row++; 

mvprintw (row++, column, "  Press  Escape  To  Shutdown  Anytime"); 
break; 
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case  1 : 

column++; 


mvprintw ( 1 , column. 

"Primitive  Manipulator  Screen"); 

mvprintw (3, column. 

"PM  State: 

%s" , 

jausStateString (pmGetState ( )  )  )  ; 

mvprintw ( 4 , column , 

"PM  Instance  ID: 

%3.0u' 

",  pmGetlnstanceld ( ) 

mvprintw (5, column. 

"PM  Component  ID: 

%3.0u' 

™ ,  pmGetCompId ( ) ) ; 

mvprintw ( 6 , column , 

"PM  Node  ID: 

%3 . Ou" , 

pmGetNodeld ( ) ) ; 

mvprintw ( 7 , column , 

"PM  Subsystem  ID: 

%3.0u' 

",  pmGetSubsystemld ( 

mvprintw ( 8 , column. 

"PM  Update  Rate: 

%5 . 2f ’ 

" ,  pmGetUpdateRate ( ) 

mvprintw ( 9 , column , 

"Interface  Update 

Rate : 

%  5 . 2  f  " , 

getlnterfaceUpdateRate () ) ; 

mvaddstr ( 10 , column. 


mvaddstr (1, 65, "Commanded  Effort") ; 
mvaddstr (3, 65, "Joint  1:"); 
mvaddstr (4, 65, "Joint  2:"); 
mvaddstr (5, 65, "Joint  3:"); 
mvaddstr ( 6, 65, "Joint  4:"); 
mvaddstr (7 , 65, "Joint  5:"); 
mvaddstr ( 8 , 65, "Joint  6:"); 

effort  =  pmSet JointEf fort ( )  ; 
mvprintw (3, 75,"%  6.2f", ef fort-> j ointEf fort [ 0 ] ) ; 
mvprintw (4, 75,"%  6.2f", ef fort-> j  ointEf fort [ 1 ] ) ; 
mvprintw (5, 75,"%  6 . 2f " , ef fort-> j  ointEf fort [2 ] ) ; 
mvprintw (6, 75,"%  6 . 2f ", ef fort->j ointEf fort [3] ) ; 
mvprintw (7, 75,"%  6.2f", ef fort-> j  ointEf fort [ 4 ] ) ; 
mvprintw (8, 75,"%  6 . 2f " , ef fort-> j  ointEf fort [5] ) ; 

mvaddstr ( 11 , column, "Manipulator  Specifications" ) ; 

mvaddstr (16, 1, "Joint  1:"); 

mvaddstr ( 17 , 1 , "Joint  2:"); 

mvaddstr ( 18 , 1 , "Joint  3:"); 

mvaddstr (19, 1, "Joint  4:"); 

mvaddstr (20 , 1 , "Joint  5:"); 

mvaddstr (21 , 1 , "Joint  6:"); 

specs  =  pmReportManipulatorSpecif ications ( ) ; 
mvaddstr (13,9, "Type") ; 

mvprintw (16, 10, "%  1 . Od", specs->j ointSpecif ications [0] . jointType) ; 
mvprintw (17,10,"%  l.Od", specs -> j ointSpecif ications [ 1 ] . j ointType) ; 
mvprintw (18, 10, "%  1 . Od", specs->j ointSpecif ications [2] .jointType) ; 
mvprintw (19, 10, "%  1 . Od", specs->j ointSpecif ications [3] .jointType) ; 
mvprintw (20, 10, "%  1 . Od", specs->j ointSpecif ications [4] .jointType) ; 
mvprintw (21, 10, "%  1 . Od", specs-> j ointNtype) ; 

mvaddstr (13, 15, "Lnk_L" ) ; 
mvaddstr (14, 16, " (mm) ") ; 

mvprintw (16, 15, "%  4 . Of", specs->j ointSpecif ications [0] . linkLength) ; 
mvprintw (17, 15, "%  4 . Of", specs->j ointSpecif ications [1] . linkLength) ; 
mvprintw (18, 15, "%  4 . Of", specs->j ointSpecif ications [2] .linkLength) ; 
mvprintw (19, 15, "%  4 . Of", specs->j ointSpecif ications [3] . linkLength) ; 
mvprintw (20, 15, "%  4 . Of", specs->j ointSpecif ications [4] . linkLength) ; 


mvaddstr (13,21, "Twist_Ang" ) ; 
mvaddstr (14,23, " (deg)  " )  ; 


mvprintw (16,22, 

"% 

6.  If" 

,  specs->jointSpecifications 

[0] 

. twistAngle 

* 

RAD_ 

TO_ 

DEG 

/ 

1000) 

; 

mvprintw (17,22, 

"% 

6.  If" 

, specs->jointSpecifications 

[1] 

. twistAngle 

* 

RAD_ 

_TO_ 

DEG 

/ 

1000) 

; 

mvprintw (18,22, 

"% 

6.  If" 

,  specs->jointSpecifications 

[2] 

. twistAngle 

* 

RAD_ 

_TO_ 

DEG 

/ 

1000) 

; 

mvprintw (19,22, 

"% 

6.  If" 

, specs->jointSpecifications 

[3] 

. twistAngle 

* 

RAD_ 

TO_ 

DEG 

/ 

1000) 

; 

mvprintw (20,22, 

"% 

6.  If" 

,  specs->jointSpecifications 

[4] 

. twistAngle 

* 

RAD 

TO 

DEG 

/ 

1000) 

; 
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mvaddstr (13,31, "Offset" ) ; 
mvaddstr (14,32, " (mm) " ) ; 
mvprintw (16, 31, "%  4.0f",specs- 
> joint Specifications [0] . j ointOff setOr Angle) ; 

mvprintw ( 17 , 31 , "%  4 . Of " , specs- 
>jointSpecifications [1] . j ointOff setOrAngle) ; 

mvprintw ( 18 , 31 , "%  4 . Of " , specs- 
>jointSpecifications [2] . j ointOff setOrAngle) ; 

mvprintw (19, 31, "%  4.0f",specs- 
> joint Specifications [3] . j ointOff setOrAngle) ; 

mvprintw (20 , 31 , "%  4.0f",specs- 
>jointSpecifications [4] . j ointOff setOrAngle) ; 

mvprintw (21, 31, "%  4 . Of", specs->j ointNoff setOrAngle) ; 


*  Jl_RAD_TO_ENC  / 

*  J2_RAD_TO_ENC  / 

*  J3_RAD_TO_ENC  / 

*  J4_RAD_TO_ENC  / 

*  J5_RAD_TO_ENC  / 
1000)  ; 


mvaddstr (13,38, "Pos_Limit" ) ; 
mvaddstr (14,40," (enc) " ) ; 

mvprintw (16, 39, "%7 . Of", specs-> j ointSpecif ications [0] . j ointMaximumValue 
1000)  ; 

mvprintw (17, 39, "%7 . Of", specs->j ointSpecif ications  [1]  . j ointMaximumValue 
1000)  ; 

mvprintw (18, 39, "%7 . Of", specs->j ointSpecif ications [2] . j ointMaximumValue 
1000)  ; 

mvprintw (19, 39, "%7 . Of", specs->j ointSpecif ications  [3]  . j ointMaximumValue 
1000)  ; 

mvprintw (20, 39, "%7 . Of", specs->j ointSpecif ications  [4]  . j ointMaximumValue 
1000)  ; 

mvprintw (21, 39, "%7 . Of ", specs-> j ointNmaximumValue  *  J6_RAD_TO_ENC  / 


>jointSpeci 
>jointSpeci 
>jointSpeci 
>jointSpeci 
>jointSpeci 
1000) ; 


mvaddstr (13,48, "Vel_Limit" ) ; 
mvaddstr (14,48,"  (enc/s) " ) ; 
mvprintw (16, 49, "%5. Of", specs - 
f ications [ 0 ]. j ointMaximumVelocity  *  Jl_RAD_TO_ENC  /  1000) 
mvprintw (17, 49, "%5. Of", specs - 
f ications [ 1 ]. j ointMaximumVelocity  *  J2_RAD_TO_ENC  /  1000) 
mvprintw (18, 49, "%5. Of", specs - 
f ications [2 ]. j ointMaximumVelocity  *  J3_RAD_TO_ENC  /  1000) 
mvprintw (19, 49, "%5. Of", specs - 
f ications [3] . j ointMaximumVelocity  *  J4_RAD_TO_ENC  /  1000) 
mvprintw (20, 49," %5. Of", specs - 
f ications [ 4 ]. j ointMaximumVelocity  *  J5_RAD_TO_ENC  /  1000) 


mvprintw (21, 49, "%5 . Of ", specs-> j ointNmaximumVelocity  *  J6_RAD_TO_ENC  / 


mvaddstr (23, 1 , "Manipulator  Type : " ) ; 
mvaddstr (24 , 3, "PUMA  762  Serial  6DOF"); 
mvaddstr (26, 1 , "Controller  Type : " ) ; 
mvaddstr (27, 6, "GALIL  MC,  DMC2100"); 

mvaddstr (23, 27 , "Manipulator  Coordinate  System:"); 

mvaddstr (24 , 30 ,  "Position:"); 

mvaddstr (24,45,  "Orientation : " ) ; 

mvaddstr (25, 30, "X" )  ; 

mvaddstr (26,30,  "Y" )  ; 

mvaddstr (27 , 30 ,  "Z" )  ; 

mvaddstr (25,  45,  "D" )  ; 

mvaddstr (26,45, "A" ) ; 

mvaddstr (27,45, "B" ) ; 

mvaddstr (28,45, "C")  ; 

mvprintw (25,32,"%  4 . If" , specs->manipulatorCoordinateSystemX) ; 
mvprintw (26, 32, "%  4 . If", specs->manipulatorCoordinateSystemY) ; 
mvprintw (27,32,"%  4 . If " , specs->manipulatorCoordinateSystemZ) ; 
mvprintw (25,47,"%  4 . If " , specs->quaternionQcomponentD) ; 
mvprintw (26,47,"%  4 . If", specs->quaternionQcomponentA) ; 
mvprintw (27,47,"%  4 . If " , specs->quaternionQcomponentB) ; 
mvprintw (28,47,"%  4 . If " , specs->quaternionQcomponentC) ; 

mvaddstr (11, 67, "Current  Effort") ; 
mvaddstr ( 13, 65, "Joint  1:"); 
mvaddstr ( 14 , 65, "Joint  2:"); 
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mvaddstr ( 15, 65, " Joint  3:"); 
mvaddstr (16, 65, " Joint  4:"); 
mvaddstr (17, 65, " Joint  5:"); 
mvaddstr ( 18 , 65, "Joint  6:"); 

effort  =  pmReport JointEf fort ( ) ; 
mvprintw (13,75,"%  6.2f", ef fort-> j ointEf fort [ 0 ] ) ; 
mvprintw (14,75,"%  6.2f", ef fort-> j  ointEf fort [ 1 ] ) ; 
mvprintw (15,75,"%  6 . 2f " , ef f ort-> j ointEf fort [2 ] ) ; 
mvprintw (16,75,"%  6.2f", ef fort-> j  ointEf fort [3] ) ; 
mvprintw (17,75,"%  6 . 2f " , ef f ort-> j ointEf fort [ 4 ] ) ; 
mvprintw (18,75,"%  6.2f", ef fort-> j  ointEf fort [5] ) ; 

break; 

case  2 : 

column++; 

mvprintw ( 1 , column, "Manipulator  Joint  Position  and  Velocity  Sensors 
Screen:",  mcGetNameO  ); 

mvprintw (3, column, "MJPS  State:  %s", 

jausStateString (mjpsGetState ( ) )  ); 

mvprintw ( 4 , column, "MJPS  Instance  ID:  %3.0u",  mjpsGetlnstanceld ( ) )  ; 
mvprintw (5, column, "MJPS  Component  ID:  %3.0u",  mjpsGetCompId ( ) ) ; 
mvprintw ( 6, column, "MJPS  Node  ID:  %3.0u", 

mjpsGetNodeld ()); 

mvprintw (7 , column, "MJPS  Subsystem  ID:  %3.0u", 

mjpsGetSubsystemld ( ) ) ; 


mvprintw ( 8 , column, "MJPS  Update  Rate: 

%  5 . 2  f  " , 

.  mjpsGetUpdateRate ( 

mvprintw (3,40, 

"MJVS 

State : 

%s" , 

jausStateString (mj vsGetState  ( ) )  ) ; 

mvprintw (4,40, 

"MJVS 

Instance  ID: 

%3 . Ou" , 

.  mj vsGetlnstanceld ( 

mvprintw (5,40, 

"MJVS 

Component  ID: 

%3 . Ou" , 

.  mj vsGetCompId ( ) )  ; 

mvprintw (6,40, 

"MJVS 

Node  ID: 

%3.0u", 

.  mj vsGetNodeld ( )  )  ; 

mvprintw (7,40, 

"MJVS 

Subsystem  ID: 

%3 . Ou" , 

mj vsGetSubsystemld ( ) ) 

mvprintw (8,40, 

"MJVS 

Update  Rate: 

%  5 . 2  f  " , 

.  mj vsGetUpdateRate ( 

mvaddstr ( 10 , column, " - 

mvaddstr ( 11 , column, "Current  position  in:"); 

mvaddstr ( 13, 13, "Joint  1:"); 
mvaddstr ( 14 , 13, "Joint  2:"); 
mvaddstr ( 15, 13, "Joint  3:"); 
mvaddstr (16, 13, "Joint  4:"); 
mvaddstr ( 17 , 13, "Joint  5:"); 
mvaddstr ( 18 , 13, "Joint  6:"); 

position  =  mjpsReport JointPosition ( ) ; 
mvprintw (11,25, " radians " )  ; 

mvprintw (13,25,"%  6 . 4f ",  posit ion->j oint Posit ion [0] ) ; 
mvprintw (14,25,"%  6 . 4f ",  posit ion->j oint Posit ion [1] ) ; 
mvprintw (15, 25, "%  6 . 4f " , position-> j ointPosition [2] ) ; 
mvprintw (16, 25, "%  6 . 4f" , position->j ointPosition [3] ) ; 
mvprintw (17,25,"%  6 . 4f ",  posit ion->j ointPosition [4] ) ; 
mvprintw (18,25,"%  6 . 4f ",  posit ion->j ointPosition [5] ) ; 

mvprintw (11,40, "degrees" )  ; 

mvprintw ( 13, 40 , "%  6 . 2f" , position->j ointPosition [ 0 ]  *  RAD_TO_DEG) ; 
mvprintw ( 14 , 40 , "%  6 . 2f" , position->j ointPosition [ 1 ]  *  RAD_TO_DEG) ; 
mvprintw ( 15, 40 , "%  6 . 2f" , position->j ointPosition [2 ]  *  RAD_TO_DEG) ; 
mvprintw ( 16, 40 , "%  6 . 2f " , position->j ointPosition [3]  *  RAD_TO_DEG) ; 
mvprintw ( 17 , 40 , "%  6 . 2f" , position->j ointPosition [ 4 ]  *  RAD_TO_DEG) ; 
mvprintw ( 18 , 40 , "%  6 . 2f " , position->j ointPosition [5]  *  RAD_TO_DEG) ; 


mvprintw (11,55, "enc_cnts" ) ; 

mvprintw ( 13, 55, "%  6 . Of ", position->j ointPosition [ 0 ]  * 

Jl_RAD_TO_ENC) ; 
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mvprintw ( 14 , 55, "%  6 . Of " , position-> j ointPosition [ 1 ]  *  J2_RAD_TO_ENC) ; 
mvprintw ( 15, 55, "%  6 . Of ", position->j ointPosition [2 ]  *  J3_RAD_TO_ENC) ; 
mvprintw (16, 55, "%  6 . Of " , position->j ointPosition [3]  *  J4_RAD_TO_ENC) ; 
mvprintw ( 17 , 55, "%  6 . Of ", position->j ointPosition [ 4 ]  *  J5_RAD_TO_ENC) ; 
mvprintw ( 18 , 55, "%  6 . Of " , position->j ointPosition [5]  *  J6_RAD_TO_ENC) ; 


mvaddstr (20 , column, "Current  velocity  in:"); 

mvaddstr (22 , 13, " Joint  1:"); 
mvaddstr (23, 13, "Joint  2:"); 
mvaddstr (24 , 13, "Joint  3:"); 
mvaddstr (25, 13, "Joint  4:"); 
mvaddstr (26, 13, "Joint  5:"); 
mvaddstr (27 , 13, "Joint  6:"); 

velocity  =  mj vsReport JointVelocity ( ) ; 

mvprintw (20,25, " radians / s " )  ; 

mvprintw (22,25,"%  6.4f", velocity-> j ointVelocity [ 0 ] ) ; 
mvprintw (23,25,"%  6.4f", velocity-> j  ointVelocity [ 1 ] ) ; 
mvprintw (24,25,"%  6 . 4f " , velocity-> j ointVelocity [2 ] ) ; 
mvprintw (25,25,"%  6 . 4f " , velocity->j ointVelocity [3] ) ; 
mvprintw (26,25,"%  6 . 4f " , velocity-> j  ointVelocity [ 4 ] ) ; 
mvprintw (27,25,"%  6.4f", velocity-> j ointVelocity [5] ) ; 

mvprintw (20,40, "degrees/s" )  ; 

mvprintw (22 , 40 , "%  6 . 2f" , velocity->j ointVelocity [ 0 ]  *  RAD_TO_DEG) ; 
mvprintw (23, 40 , "%  6 . 2f" , velocity->j ointVelocity [ 1 ]  *  RAD_TO_DEG) ; 
mvprintw (24 , 40 , "%  6 . 2f" , velocity->j ointVelocity [2 ]  *  RAD_TO_DEG) ; 
mvprintw (25, 40 , "%  6 . 2f " , velocity->j ointVelocity [3]  *  RAD_TO_DEG) ; 
mvprintw (26, 40, "%  6 . 2f" , velocity->j ointVelocity [ 4 ]  *  RAD_TO_DEG) ; 
mvprintw (27 , 40 , "%  6 . 2f " , velocity->j ointVelocity [5]  *  RAD_TO_DEG) ; 


mvprintw (20,55, "enc_cnts/s" ) ; 

mvprintw (22 , 55, "%  6 . Of ", velocity->j ointVelocity [ 0 ]  * 

Jl_RAD_TO_ENC) ; 

mvprintw (23, 55, "%  6 . Of ", velocity->j ointVelocity [ 1 ]  *  J2_RAD_TO_ENC) ; 
mvprintw (24 , 55, "%  6 . Of ", velocity->j ointVelocity [2 ]  *  J3_RAD_TO_ENC) ; 
mvprintw (25, 55, "%  6 . Of " , velocity->j ointVelocity [3]  *  J4_RAD_TO_ENC) ; 
mvprintw (26, 55, "%  6 . Of ", velocity->j ointVelocity [ 4 ]  *  J5_RAD_TO_ENC) ; 
mvprintw (27 , 55, "%  6 . Of " , velocity->j ointVelocity [5]  *  J6_RAD_TO_ENC) ; 


break; 
case  3 : 

column++; 

row++; 

mvprintw (row++. 
Screen",  mcGetNameO  ); 

row++; 

mvprintw (row++, 

jausStateString (mj  ftsGetState ( ) )  ) ; 

mvprintw (row++, 

mj  ftsGetlnstanceld ( ) ) ; 

mvprintw (row++, 

mj  f tsGetCompId ( ) ) ; 

mvprintw (row++, 

mj  f tsGetNodeld ( ) ) ; 

mvprintw (row++, 
mj  ftsGetSubsystemld ( ) ) ; 

mvprintw (row++, 

mj  f tsGetUpdateRate ( ) ) ; 

row++; 

mvaddstr (row++. 


"Manipulator  Joint  Force/Torque  Sensor 


"MJFTS  State:  %s", 

"MJFTS  Instance  ID:  %3.0u", 

"MJFTS  Component  ID:  %3.0u", 

"MJFTS  Node  ID:  %3.0u", 

"MJFTS  Subsystem  ID:  %3.0u", 

"MJFTS  Update  Rate:  %5.2f". 


mvaddstr (row++, column, "Current  torque  in:"); 
row++; 

mvaddstr ( 13, 11 , "Joint  1:"); 
mvaddstr ( 14 , 11 , "Joint  2:"); 
mvaddstr ( 15, 11 , "Joint  3:"); 


mvaddstr (16, 11, " Joint  4 
mvaddstr ( 17 , 11 , " Joint  5 
mvaddstr ( 18 , 11 , "Joint  6 
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forcetorque  =  mj f tsReport JointForceTorque ( ) ; 
mvprintw (11,25, "N-m" ) ; 

mvprintw (13,23,"%  6 . 4f " , f orcetorque-> j ointForceTorque [ 0 ] ) ; 
mvprintw (14,23,"%  6 . 4f " , f orcetorque-> j  ointForceTorque [ 1 ] ) ; 
mvprintw (15,23,"%  6 . 4f " , f orcetorque-> j  ointForceTorque  [  2 ] ) ; 
mvprintw (16,23,"%  6 . 4f " , f orcetorque-> j  ointForceTorque [ 3 ] ) ; 
mvprintw (17,23,"%  6 . 4f " , f orcetorque-> j  ointForceTorque [ 4 ] ) ; 
mvprintw (18,23,"%  6 . 4f " , f orcetorque-> j  ointForceTorque [ 5 ] ) ; 


mvprintw (11,38 
mvprintw (13,38 

>jointForceTorque [0] /FTLB_TO_NM) ; 

mvprintw (14,38,"% 
mvprintw  (15,38,"% 
mvprintw (16,38,"% 
mvprintw (17,38,"% 
mvprintw (18,38,"% 


,  "ft_lbs" ) ; 

,  "%  6 . 2f " , forcetorque- 

6 . 2f " , f or cetorque->j ointForceTorque [ 1 ] 
6 . 2f " , f or cetorque->j ointForceTorque [2 ] 
6 . 2f " , f or cetorque->j ointForceTorque [3] 
6 . 2f " , f or cetorque->j ointForceTorque [4 ] 
6 . 2f " , f or cetorque->j ointForceTorque [5] 


/FTLB_TO_NM) ; 
/FTLB_T0_NM) ; 
/FTLB_T0_NM) ; 
/FTLB_T0_NM) ; 
/FTLB_T0_NM) ; 


break; 


case  4 : 

column+l; 

mvprintw ( 1 , column, "Manipulator  Joint  Positions  Driver  Screen", 


mvprintw (3, column,  ' 

"MJPD 

State : 

%s" , 

jausStateString (mjpdGetState ( ) )  ) ; 

mvprintw ( 4 , column ,  ' 

"MJPD 

Instance  ID: 

%3 . Ou" 

,  mjpdGetlnstanceld ( )  )  ; 

mvprintw (5, column,  ' 

"MJPD 

Component  ID: 

%3.0u" 

,  mjpdGetCompId ( ) ) ; 

mvprintw ( 6 , column ,  ' 

"MJPD 

Node  ID; 

%3 . Ou" , 

mjpdGetNodeld ( ) ) ; 

mvprintw ( 7 , column ,  ' 

"MJPD 

Subsystem  ID; 

%3 . Ou" 

mjpdGetSubsystemld ( ) ) 

mvprintw ( 8 , column,  ' 
mvaddstr ( 10 , column, 

"MJPD 

Update  Rate: 

%  5 . 2  f  " 

,  mjpdGetUpdateRate ( ) ) ; 

mvaddstr (1,63, "Commanded  Position" ) ; 
mvaddstr (3, 64, "Joint  1:"); 
mvaddstr ( 4 , 64 , "Joint  2:"); 
mvaddstr (5, 64, "Joint  3:"); 
mvaddstr ( 6 , 64 , "Joint  4:"); 
mvaddstr (7, 64, "Joint  5:"); 
mvaddstr ( 8 , 64 , "Joint  6:"); 


position  =  mjpdSet JointPosition ( ) ; 

mvprintw (3, 75, "%  6 . Of " , position-> j ointPosition [ 0 ]  *  Jl_RAD_TO_ENC); 
mvprintw ( 4 , 75 , "%  6 . Of ", position->j ointPosition [ 1 ]  *  J2_RAD_TO_ENC) ; 
mvprintw (5, 75, "%  6 . Of ", position->j ointPosition [2 ]  *  J3_RAD_TO_ENC); 
mvprintw ( 6 , 75 , "%  6 . Of ", position->j ointPosition [ 3 ]  *  J4_RAD_TO_ENC); 
mvprintw ( 7 , 75 , "%  6 . Of ", position->j ointPosition [ 4 ]  *  J5_RAD_TO_ENC); 
mvprintw ( 8 , 75 , "%  6 . Of ", position->j ointPosition [ 5 ]  *  J6_RAD_TO_ENC) ; 


mvaddstr (11, column, "Queried  Information 

mvaddstr (13, 13, "Joint  1:"); 

mvaddstr ( 14 , 13 , "Joint  2:"); 

mvaddstr (15, 13, "Joint  3:"); 

mvaddstr (16, 13, "Joint  4:"); 

mvaddstr (17, 13, "Joint  5:"); 

mvaddstr (18, 13, "Joint  6:"); 

effort  =  mjpdReport JointEf fort ( ) ; 

mvaddstr (11,26, "effort" )  ; 
mvprintw (13,24,"%  7.2f",effort->j ointEf f ort [ 0 ] ) ; 
mvprintw (14,24,"%  7.2f",effort->j ointEf fort [ 1 ] ) ; 
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mvprintw (15,24, 
mvpr intw (16,24, 
mvprintw (17,24, 
mvprintw (18,24, 


7 . 2f " , ef fort-> j ointEf fort [2 ] )  ; 
7 . 2f ", ef fort->j ointEf fort [3]  )  ; 
7 . 2f " , ef fort-> j  ointEf fort [ 4  ]  )  ; 
7 . 2f ", ef fort->j ointEf fort [5]  )  ; 


J1_RAD_T0_ENC) ; 


position  =  mjpdReport JointPosition ( ) ; 
mvaddstr (11,35, "position" )  ; 

mvprintw ( 13, 36, "%  6 . Of " , position-> j ointPosition [ 0 ]  * 


mvprintw (14,36, 
mvprintw (15,36, 
mvprintw (16,36, 
mvprintw (17,36, 
mvprintw (18,36, 


% 

6.  Of" 

, posit ion-> j ointPosition 

i] 

* 

J2 

RAD_ 

TO_ 

ENC)  ; 

% 

6.  Of" 

, posit ion-> j ointPosition 

2] 

* 

J3_ 

RAD_ 

TO 

ENC)  ; 

% 

6.  Of" 

, posit ion-> j ointPosition 

3] 

* 

J4 

_RAD 

"to 

ENC)  ; 

% 

6.  Of" 

, posit ion-> j ointPosition 

4] 

* 

J5_ 

_RAD_ 

TO 

ENC)  ; 

% 

6.  Of" 

, posit ion-> j ointPosition 

5] 

* 

J6^ 

_RAD 

TO 

ENC)  ; 

mvaddstr ( 11 , 65, "Resulting  Effort" ) ; 
mvaddstr ( 13, 64 , "Joint  1:"); 
mvaddstr ( 14 , 64 , "Joint  2:"); 
mvaddstr ( 15, 64 , "Joint  3:"); 
mvaddstr ( 16, 64 , "Joint  4:"); 
mvaddstr ( 17 , 64 , "Joint  5:"); 
mvaddstr ( 18 , 64 , "Joint  6:"); 


effort  = 
mvprintw (13, 
mvprintw ( 14 , 
mvprintw (15, 
mvprintw (16, 
mvprintw ( 17 , 
mvprintw ( 18 , 

break; 

case  5 : 

column++1 

mvprintw 

mcGetName ( )  ) ; 

mvprintw 

jausStateString (meepdGetState ( ) 
mvprintw 

meepdGetlnstanceld ( ) ) ; 

mvprintw 

meepdGetCompId ( ) )  ; 

mvprintw 

meepdGetNodeld ( ) ) ; 

mvprintw 

meepdGetSubsystemld ( ) ) ; 

mvprintw 

meepdGetUpdateRate ( ) )  ; 

mvaddstr 


mjpdSet JointEf fort ()  ; 


73, 

73, 

73, 

73, 

73, 

73, 


] . 2f " , ef fort->j ointEf fort [0] ) 
] . 2f ", effort->j ointEf fort [1] ) 
] . 2f " , ef fort-> j  ointEf fort [2 ] ) 
] . 2f " , ef fort->j ointEf fort [3] ) 
] . 2f" , effort->j ointEf fort [4] ) 
] . 2f " , ef fort->j ointEf fort [5] ) 


( 1 , column, "Manipulator  End-Effector  Pose  Driver  Screen" 

(3, column, "MEEPD  State:  %s", 

)  )  ; 

( 4 , column, "MEEPD  Instance  ID:  %3.0u", 

(5, column, "MEEPD  Component  ID:  %3.0u", 

(  6, column, "MEEPD  Node  ID:  %3.0u", 

(7 , column, "MEEPD  Subsystem  ID:  %3.0u", 

( 8 , column, "MEEPD  Update  Rate:  %5.2f", 

( 10 , column, " - 


mvaddstr (1,56, "Commanded  Pose : " ) ; 
mvaddstr (3,56, "position" ) ; 
mvaddstr (5, 56, "X : " )  ; 
mvaddstr (6, 56, "Y: " )  ; 
mvaddstr (7 , 56,  "Z  :  " )  ; 

mvaddstr (3,70, "orientation" )  ; 
mvaddstr (5, 70,  "D: " )  ; 
mvaddstr ( 6 , 7  0 , " A : " ) ; 
mvaddstr (7 , 70 , "B : " )  ; 
mvaddstr ( 8 , 70 , "C : " )  ; 

endef fectorpose  =  meepdSetEndEf fectorPose () ; 
mvprintw (5, 59,"%  6 . 2f " , endef fectorpose->X* 1000 ) ; 
mvprintw (6, 59,"%  6.2f", endef fectorpose->Y* 1000 ) ; 
mvprintw (7, 59,"%  6.2f", endef fectorpose->Z* 1000 ) ; 


mvprintw (5, 74 , "%  6 . 4f " , endef fectorpose->quaternionQcomponentD) ; 
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mvprintw ( 6, 74 , "%  6 . 4f " , endef fectorpose->quaternionQcomponentA) ; 
mvprintw (7 , 74 , "%  6 . 4f " , endef fectorpose->quaternionQcomponentB) ; 
mvprintw ( 8 , 74 , "%  6 . 4f " , endef fectorpose->quaternionQcomponentC) ; 


mvaddstr ( 11 , column, "Queried  Information : " ) ; 

mvaddstr ( 13, 13, " Joint  1:"); 

mvaddstr ( 14 , 13, "Joint  2:"); 

mvaddstr ( 15, 13, "Joint  3:"); 

mvaddstr (16, 13, "Joint  4:"); 

mvaddstr ( 17 , 13, "Joint  5:"); 

mvaddstr ( 18 , 13, "Joint  6:"); 


effort  =  meepdReport JointEf fort () ; 
mvaddstr (11,26, "effort" )  ; 
mvprintw (13,24,"%  7.2f", ef fort-> j ointEf fort [ 0 ] ) ; 
mvprintw (14,24,"%  7.2f", ef fort-> j  ointEf fort [ 1 ] ) ; 
mvprintw (15,24,"%  7 . 2f " , ef fort-> j  ointEf fort [2 ] ) ; 
mvprintw (16,24,"%  7 . 2f " , ef fort-> j ointEf fort [3] ) ; 
mvprintw (17,24,"%  7 . 2f " , ef fort-> j  ointEf fort [4 ] ) ; 
mvprintw (18,24,"%  7.2f", ef fort-> j  ointEf fort [5] ) ; 


J1_RAD_T0_ENC) ; 


position  =  meepdReport JointPosition () ; 
mvaddstr (11,35, "position" )  ; 

mvprintw ( 13, 35, "%  7 . Of " , position-> j ointPosition [ 0 ]  * 


mvprintw ( 14 , 35, "%  7 . Of ", position->j ointPosition [ 1 ]  *  J2_RAD_T0_ENC) ; 
mvprintw ( 15, 35, "%  7 . Of ", position->j ointPosition [2 ]  *  J3_RAD_T0_ENC) ; 
mvprintw (16, 35, "%  7 . Of " , position->j ointPosition [3]  *  J4_RAD_T0_ENC) ; 
mvprintw ( 17 , 35, "%  7 . Of ", position->j ointPosition [ 4 ]  *  J5_RAD_T0_ENC) ; 
mvprintw ( 18 , 35, "%  7 . Of " , position->j ointPosition [5]  *  J6_RAD_T0_ENC) ; 


toolpoint  =  meepdReportToolPoint ( )  ; 
mvaddstr (20,27, "pose" ) ; 

mvaddstr (22, 19,  "X: ")  ; 
mvaddstr (23, 19,  "Y : " )  ; 
mvaddstr (24, 19,  "Z  :  ")  ; 


mvaddstr (22,33, 

"D 

")  ; 

mvaddstr (23, 33, 

"A 

")  ; 

mvaddstr (24,33, 

"B 

")  ; 

mvaddstr (25,33, 

"C 

")  ; 

mvprintw (22,24, 

"% 

4 . 2  f  " 

,  toolpoint->X)  ; 

mvprintw (23, 24, 

"% 

4 . 2  f  " 

,  toolpoint->Y)  ; 

mvprintw (24,24, 

"  % 

4.2f" 

,  toolpoint->Z)  ; 

currentOrientatior 

=  meepdGetCurrentOrientation ( 

mvprintw (22,35, 

"% 

6 . 4  f  " 

, currentOrientation 

0]  )  ; 

mvprintw (23, 35, 

"% 

6 . 4  f  " 

,  currentOrientation 

1]  )  ; 

mvprintw (24,35, 

"% 

6 . 4  f  " 

, currentOrientation 

2]  )  ; 

mvprintw (25, 35, 

"% 

6 . 4  f  " 

, currentOrientation 

3]  )  ; 

numSoln  =  cppGetNumSoln ( ) ; 

mvaddstr (22 , 55, "Number  of  solutions:"); 

mvprintw (22,79,"%  1 . Od" , numSoln) ; 

optSoln  =  cppGetOptSoln ( ) ; 

mvaddstr (24 , 55, "Optimal  solution  is:"); 

mvprintw (24,79,"  %1.0d", optSoln) ; 

totalCost  =  cppGetCost ( ) ; 
mvaddstr (26,55, "Current  cost : " ) ; 
mvprintw (26,73,"  %7 . Of " , totalCost) ; 

mvaddstr (11, 65, "Resulting  Effort") ; 
mvaddstr ( 13, 64 , "Joint  1:"); 
mvaddstr ( 14 , 64 , "Joint  2:"); 
mvaddstr ( 15, 64 , "Joint  3:"); 
mvaddstr ( 16, 64 , "Joint  4:"); 


194 


mvaddstr ( 17 , 64 , " Joint  5:"); 
mvaddstr ( 18 , 64 , "Joint  6:"); 


effort  = 
mvprintw (13, 
mvprintw ( 14 , 
mvprintw (15, 
mvprintw (16, 
mvprintw ( 17 , 
mvprintw ( 18 , 


meepdSet JointEf fort () 


73, 

73, 

73, 

73, 

73, 

73, 


) . 2f " , ef fort-> j  ointEf fort 
] . 2f " , ef fort-> j  ointEf fort 
] . 2f " , ef fort-> j  ointEf fort 
] . 2f " , ef fort-> j  ointEf fort 
$ . 2f " , ef fort-> j  ointEf fort 
] . 2f " , ef fort-> j  ointEf fort 


[0]  )  ; 
[1]  )  ; 
[2]  )  ; 

[3]  )  ; 

[4]  )  ; 

[5]  )  ; 


break; 


case  6 : 

column++; 

mvprintw ( 1 , column, "Manipulator  Joint  Velocities  Driver  Screen", 

mcGetName ( )  ) ; 


mvprintw (3, column, "MJVD 
jausStateString (mj vdGetState  ( ) )  ) ; 

mvprintw ( 4 , column , "MJVD 
mvprintw (5, column, "MJVD 
mvprintw ( 6 , column , "MJVD 

mj vdGetNodeld ( ) )  ; 

mvprintw ( 7 , column , "MJVD 

mj vdGetSubsystemld ( ) ) ; 

mvprintw ( 8 , column, "MJVD 
mvaddstr ( 10 , column, " - 


State : 

%s" , 

Instance  ID: 
Component  ID: 
Node  ID: 

%3.0u" 

%3.0u" 

,  mj vdGetlnstanceld ( 
,  mj vdGetCompId ( ) ) ; 
%3 . Ou" , 

Subsystem  ID: 

%3.0u" 

Update  Rate: 

%  5 . 2  f  " 

,  mj vdGetUpdateRate ( 

mvaddstr ( 1 , 63, "Commanded  Velocity" ) ; 
mvaddstr (3, 64 , "Joint  1:"); 
mvaddstr ( 4 , 64 , "Joint  2:"); 
mvaddstr (5, 64 , "Joint  3:"); 
mvaddstr ( 6, 64 , "Joint  4:"); 
mvaddstr (7 , 64 , "Joint  5:"); 
mvaddstr ( 8 , 64 , "Joint  6:"); 


velocity  =  mjvdSet JointVelocity () ; 

mvprintw  (3, 75,  "%  6  .  Of  " ,  velocity-> j ointVelocity  [ 0 ]  *  Jl_RAD__TO_ENC)  ; 
mvprintw (4, 75, "%  6 . Of ", velocity->j ointVelocity [ 1 ]  *  J2_RAD_TO_ENC) ; 
mvprintw (5, 75, "%  6 . Of ", velocity->j ointVelocity [2 ]  *  J3_RAD_TO_ENC) ; 
mvprintw ( 6, 75, "%  6 . Of " , velocity->j ointVelocity [3]  *  J4_RAD_TO_ENC) ; 
mvprintw (7 , 75, "%  6 . Of ", velocity->j ointVelocity [ 4 ]  *  J5_RAD_TO_ENC) ; 
mvprintw ( 8 , 75, "%  6 . Of " , velocity->j ointVelocity [5]  *  J6_RAD_TO_ENC) ; 


mvaddstr ( 11 , column, "Queried  Information : " ) ; 

mvaddstr ( 13, 13, "Joint  1:"); 

mvaddstr ( 14 , 13, "Joint  2:"); 

mvaddstr ( 15, 13, "Joint  3:"); 

mvaddstr (16, 13, "Joint  4:"); 

mvaddstr ( 17 , 13, "Joint  5:"); 

mvaddstr ( 18 , 13, "Joint  6:"); 

effort  =  mjvdReport JointEf fort () ; 

mvaddstr (11, 26, "effort") ; 
mvprintw (13,24,"%  7 . 2f " , ef fort-> j  ointEf fort [ 0 ] ) ; 
mvprintw (14,24,"%  7 . 2f " , ef fort-> j  ointEf fort [ 1 ] )  ; 
mvprintw (15,24,"%  7.2f", effort->j ointEf fort [2] ) ; 
mvprintw (16,24,"%  7 . 2f " , ef fort-> j  ointEf fort [3] ) ; 
mvprintw (17,24,"%  7 . 2f " , ef fort-> j  ointEf fort [ 4 ] )  ; 
mvprintw (18,24,"%  7.2f", ef fort-> j  ointEf fort [5] ) ; 

velocity  =  mjvdReport JointVelocity () ; 

mvaddstr (11,35, "velocity" ) ; 

mvprintw ( 13, 36, "%  6 . Of ", velocity->j ointVelocity [ 0 ]  * 

Jl_RAD_TO_ENC) ; 

mvprintw ( 14 , 36, "%  6 . Of ", velocity->j ointVelocity [ 1 ]  *  J2_RAD_TO_ENC) ; 
mvprintw ( 15, 36, "%  6 . Of ", velocity->j ointVelocity [2 ]  *  J3_RAD_TO_ENC) ; 
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mvpr intw (16,36,"% 
mvpr intw (17,36,"% 
mvpr intw (18,36,"% 


6 . Of " , velocity-> j ointVelocity [3]  *  J4_RAD_T0_ENC) ; 
6 . Of " , velocity-> j ointVelocity [ 4 ]  *  J5_RAD_T0_ENC) ; 
6 . Of " , velocity->j ointVelocity [5]  *  J6_RAD_T0_ENC) ; 


mvaddstr (11, 65, "Resulting  Effort") ; 
mvaddstr ( 13, 64 , " Joint  1:"); 
mvaddstr ( 14 , 64 , "Joint  2:"); 
mvaddstr ( 15, 64 , "Joint  3:"); 
mvaddstr (16, 64, "Joint  4:"); 
mvaddstr ( 17 , 64 , "Joint  5:"); 
mvaddstr ( 18 , 64 , "Joint  6:"); 


effort  = 
mvprintw (13, 
mvpr intw ( 14 , 
mvprintw (15, 
mvprintw (16, 
mvprintw ( 17 , 
mvprintw ( 18 , 


mj vdSet JointEf fort ()  ; 


75, 

75, 

75, 

75, 

75, 

75, 


6 . 2f " , ef fort-> j  ointEf fort 
6 . 2f " , ef fort-> j  ointEf fort 
6 . 2f " , ef fort-> j  ointEf fort 
6 . 2f " , ef fort-> j  ointEf fort 
6 . 2f " , ef fort-> j  ointEf fort 
6 . 2f " , ef fort-> j ointEf fort 


[0]  )  ; 
[1]  )  ; 
[2]  )  ; 

[3]  )  ; 

[4]  )  ; 

[5]  )  ; 


break; 


case  8 : 


column++; 

mvprintw ( 1 , column, "Manipulator  Joint  Move  Driver  Screen", 


mcGetName ( )  ) ; 


mvprintw (3, column, r 

'MJMD 

State : 

jausStateString (mjmdGetState ( ) )  ) ; 

mvprintw ( 4 , column , *' 

'MJMD 

Instance  ID: 

%3 . Ou" , 

mvprintw (5, column, *' 

'MJMD 

Component  ID: 

%3 . Ou" , 

mvprintw ( 6 , column , *' 

'MJMD 

Node  ID: 

mjmdGetNodeld ( ) ) ; 

mvprintw ( 7 , column , r 

'MJMD 

Subsystem  ID: 

%3 . Ou" , 

mjmdGetSubsystemld ( ) ) 

mvprintw  ( 8 ,  column,  *' 
mvaddstr ( 10 , column. 

'MJMD 

Update  Rate: 

%  5 . 2  f  " , 

)  ; 


%s" , 


mjmdGetlnstanceld ( ) ) ; 
mjmdGetCompId ( ) ) ; 

%3 .  Ou" , 


mjmdGetUpdateRate () ) ; 


mvaddstr ( 1 , 65, "Commanded  Motion" ) ; 
mvaddstr (3, 64 , "Joint  1:"); 
mvaddstr ( 4 , 64 , "Joint  2:"); 
mvaddstr (5, 64 , "Joint  3:"); 
mvaddstr ( 6, 64 , "Joint  4:"); 
mvaddstr (7 , 64 , "Joint  5:"); 
mvaddstr ( 8 , 64 , "Joint  6:"); 


posePosition  = 
mvprintw (3,75, 
mvprintw (4,75, 
mvprintw (5,75, 
mvprintw (6,75, 
mvprintw (7,75, 
mvprintw (8,75, 


mjmdGetCurrentPosePosition ( ) ; 

"%  6 . Of ", posePosition [0]  *  Jl_RAD_TO_ENC) ; 
"%  6 . Of ", posePosition [1]  *  J2_RAD_TO_ENC) ; 
"%  6 . Of ", posePosition [2]  *  J3_RAD_TO_ENC) ; 
"%  6 . Of ", posePosition [3]  *  J4_RAD_TO_ENC) ; 
"%  6 . Of ", posePosition [ 4 ]  *  J5_RAD_TO_ENC) ; 
"%  6 . Of ", posePosition [5]  *  J6_RAD_TO_ENC) ; 


mvaddstr ( 11 , column, "Commanded  Joint  Values:"); 

mvaddstr ( 13, 13, "Joint  1:"); 

mvaddstr ( 14 , 13, "Joint  2:"); 

mvaddstr ( 15, 13, "Joint  3:"); 

mvaddstr (16, 13, "Joint  4:"); 

mvaddstr ( 17 , 13, "Joint  5:"); 

mvaddstr ( 18 , 13, "Joint  6:"); 

maxVelocity  =  mjmdGetCurrentPoseMaxVelocity ( ) ; 
mvaddstr (11,26, "max_vel " ) ; 

mvprintw ( 13, 24 , "%  7 . Of ", maxVelocity [ 0 ]  /  JOINTl_ENC_TO_DEG  / 

DEG_TO_RAD) ; 

mvprintw ( 14 , 24 , "%  7 . Of ", maxVelocity [ 1 ]  *  J2_RAD_TO_ENC) ; 
mvprintw ( 15, 24 , "%  7 . Of ", maxVelocity [2 ]  *  J3_RAD_TO_ENC) ; 
mvprintw ( 16, 24 , "%  7 . Of ", maxVelocity [3]  *  J4_RAD_TO_ENC) ; 
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mvprintw ( 17 , 24 , "%  7 . Of " , maxVelocity [4 ]  *  J5_RAD_TO_ENC) ; 
mvprintw ( 18 , 24 , "%  7 . Of ", maxVelocity [5]  *  J6_RAD_TO_ENC) ; 

maxAcceleration  =  mjmdGetCurrentPoseMaxAcceleration ( ) ; 


mvaddstr (11,36 

"max  acc") ; 

mvprintw (13,36 

,  "%  7 . Of ", maxAcceleration 

[0 

]  * 

Ji_ 

RAD 

_TO_ENC) 

mvprintw (14,36,"% 

7 . Of " , maxAcceleration 

i] 

* 

J2 

RAD 

_T°_ 

ENC)  ; 

mvprintw (15,36,"% 

7 . Of " , maxAcceleration 

2] 

k 

J3_ 

"rad" 

TO_ 

ENC)  ; 

mvprintw (16,36,"% 

7 . Of " , maxAcceleration 

3] 

k 

J4 

RAD 

TO_ 

ENC)  ; 

mvprintw (17,36,  " % 

7 . Of " , maxAcceleration 

4] 

k 

J5 

RAD_ 

TO_ 

ENC)  ; 

mvprintw (18,36,"% 

7 . Of " , maxAcceleration 

5] 

k 

J6 

RAD 

TO_ 

ENC)  ; 

maxDeceleration  =  mjmdGetCurrentPoseMaxDeceleration ( ) ; 
mvaddstr (11,46, "max_dec") ; 

mvprintw (13, 46, "%  7 . Of ", maxDeceleration [ 0 ]  *  J1_RAD_T0_ENC) ; 


mvprintw (14,46,"% 

7.  Of" 

, maxDeceleration 

i] 

* 

J2 

RAD_ 

_TO_ 

ENC)  ; 

mvprintw (15,46,"% 

7.  Of" 

, maxDeceleration 

2] 

* 

js; 

RAD_ 

"to_ 

ENC)  ; 

mvprintw (16,46,"% 

7.  Of" 

, maxDeceleration 

3] 

* 

J4 

RAD_ 

TO_ 

ENC)  ; 

mvprintw (17,46,"% 

7.  Of" 

, maxDeceleration 

4] 

* 

J5_ 

RAD_ 

TO_ 

ENC)  ; 

mvprintw (18,46,"% 

7.  Of" 

, maxDeceleration 

5] 

* 

J6~ 

RAD 

TO_ 

ENC)  ; 

pose  =  mjmdGetPose ( )  ; 
motion  =  mjmdSet JointMotion ( ) ; 
poseTime  =  mjmdGetPoseTime ( ) ; 
currentTime  =  mjmdGetCurrentTime ( ) ; 
mvaddstr (12,57, "Current  pose : " ) ; 
mvprintw ( 12 , 78 , "%3 . Od" , pose) ; 
mvaddstr ( 14 , 57 , "Number  of  poses:"); 
mvprintw ( 14 , 78 , "%3 . Od" , motion->numPoses) ; 
mvaddstr ( 16, 57 , "Next  pose  time:"); 
mvprintw ( 16, 78 , "%3 . Of " , poseTime) ; 
mvaddstr ( 18 , 57 , "Relative  time:  "); 
mvprintw (18,75, "%6 . 2f " , currentTime) ; 

mvaddstr (20 , column, "Queried  Information : " ) ; 

mvaddstr (22 , 13, "Joint  1:"); 

mvaddstr (23, 13, "Joint  2:"); 

mvaddstr (24 , 13, "Joint  3:"); 

mvaddstr (25, 13, "Joint  4:"); 

mvaddstr (26, 13, "Joint  5:"); 

mvaddstr (27 , 13, "Joint  6:"); 


effort  =  mjmdReport JointEf fort ( )  ; 
mvaddstr (20,26, "effort" )  ; 
mvprintw (22,24,"%  7 . 2f " , ef fort-> j ointEf fort [ 0 ] ) ; 
mvprintw (23,24,"%  7.2f", ef fort-> j ointEf fort [ 1 ] ) ; 
mvprintw (24,24,"%  7.2f", ef fort-> j ointEf fort [2 ] ) ; 
mvprintw (25,24,"%  7.2f", ef fort-> j  ointEf fort [3] ) ; 
mvprintw (26,24,"%  7.2f", ef fort-> j  ointEf fort [ 4 ] ) ; 
mvprintw (27,24,"%  7 . 2f " , ef fort-> j ointEf fort [5] ) ; 


Jl_RAD_TO_ENC) ; 


position  =  mjmdReport JointPosition () ; 
mvaddstr (20,35, "position" )  ; 

mvprintw (22 , 36, "%  6 . Of " , position-> j ointPosition [ 0 ]  * 


mvprintw (23, 36, 
mvprintw (24,36, 
mvprintw (25, 36, 
mvprintw (26,36, 
mvprintw (27,36, 


% 

6.  Of" 

, posit ion-> j ointPosition 

i] 

* 

J2 

_RAD 

TO_ 

ENC)  ; 

% 

6.  Of" 

, posit ion-> j ointPosition 

2] 

* 

J3_ 

_RAD 

TO 

ENC)  ; 

% 

6.  Of" 

, posit ion-> j ointPosition 

3] 

k 

J4 

RAD 

"to 

ENC)  ; 

% 

6.  Of" 

, posit ion-> j ointPosition 

4] 

k 

J5_ 

RAD 

TO 

ENC)  ; 

% 

6.  Of" 

, posit ion-> j ointPosition 

5] 

k 

J6~ 

RAD_ 

TO 

ENC)  ; 

Jl_RAD_TO_ENC) ; 


velocity  =  mjmdReport JointVelocity () ; 
mvaddstr (20,46, "velocity")  ; 

mvprintw (22, 46, "%  6 . Of " , velocity-> j ointVelocity [ 0 ]  * 


mvprintw (23, 46, "%  6 . Of ", velocity->j ointVelocity [ 1 ]  *  J2_RAD_TO_ENC) ; 
mvprintw (24, 46, "%  6 . Of ", velocity->j ointVelocity [2 ]  *  J3_RAD_TO_ENC) ; 
mvprintw (25, 46, "%  6 . Of " , velocity->j ointVelocity [3]  *  J4_RAD_TO_ENC) ; 
mvprintw (26, 46, "%  6 . Of ", velocity->j ointVelocity [ 4 ]  *  J5_RAD_TO_ENC) ; 
mvprintw (27, 46, "%  6 . Of " , velocity->j ointVelocity [5]  *  J6_RAD_TO_ENC) ; 
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mvaddstr (20 , 65, "Resulting  Effort" ) ; 
mvaddstr (22 , 64 , " Joint  1:"); 
mvaddstr (23, 64 , "Joint  2:"); 
mvaddstr (24 , 64 , "Joint  3:"); 
mvaddstr (25, 64 , "Joint  4:"); 
mvaddstr (26, 64 , "Joint  5:"); 
mvaddstr (27 , 64 , "Joint  6:"); 


effort  =  mjmdSet JointEf fort ( ) ; 


mvprintw (22,73, 
mvprintw (23, 73, 
mvprintw (24,73, 
mvprintw (25, 73, 
mvprintw (26,73, 
mvprintw (27,73, 


] . 2f " , ef fort->jointEf fort [0] 
] . 2f " , ef fort-> j  ointEf fort [ 1 ] 
] . 2f " , ef fort-> j ointEf fort [2 ] 
] . 2f " , ef fort->j ointEf fort [3] 
] . 2f " , ef fort-> j ointEf fort [ 4 ] 
] . 2f " , ef fort->j ointEf fort [5] 


break; 


case  9 : 

column++; 

mvprintw ( 1 , column, "Manipulator  End-Effector  DiscretePose  Driver 
Screen",  mcGetName ( )  ); 


mvprintw (3, column, 

jausStateString (meedpdGetState ( )  )  )  ; 

"MEEPD 

State : 

%s" , 

meedpdGetlnstanceld ( ) 

mvprintw ( 4 , column , 

) ; 

"MEEPD 

Instance  ID:  %3.0u" 

mvprintw (5, column. 

"MEEPD 

Component  ID: 

%3 .  Ou 

meedpdGetCompId ( ) ) ; 

mvprintw ( 6 , column , 

"MEEPD 

Node  ID: 

%3 . 0u 

meedpdGetNodeld ( ) )  ; 

meedpdGetSubsystemld ( 

mvprintw ( 7 , column , 

) ) ; 

"MEEPD 

Subsystem  ID: 

%3 .  Ou 

mvprintw ( 8 , column. 

"MEEPD 

Update  Rate:  %5.2f" 

meedpdGetUpdateRate ( )  )  ; 


mvaddstr ( 10 , column, "- 


mvaddstr (1,56, "Commanded  Pose : " ) ; 
mvaddstr (3,56, "position" ) ; 
mvaddstr (5, 56, "X : " )  ; 
mvaddstr (6, 56,  "Y :  " )  ; 
mvaddstr (7 , 56,  "Z  :  " )  ; 

mvaddstr (3,70, "orientation" )  ; 
mvaddstr (5, 70, "D; " ) ; 
mvaddstr ( 6 , 7  0 , " A : " ) ; 
mvaddstr (7 , 70 , "B : " ) ; 
mvaddstr (8, 70, "C: ") ; 

pose  =  meedpdGetPose ( ) ; 

endef fectorpathmotion  =  meedpdSetEndEf fectorPathMotion ( ) ; 

poseTime  =  meedpdGetPoseTime ( ) ; 

currentTime  =  meedpdGetCurrentTime ( ) ; 

mvaddstr (22,55, "Current  pose : " ) ; 

mvprintw (22,78,"%3.0d", pose) ; 

mvaddstr (23, 55, "Number  of  poses:"); 

mvprintw (23,78, "%3 . Od" , endef fectorpathmotion->numPoses) ; 

mvaddstr (24 , 55, "Next  pose  time:"); 

mvprintw (24,78, "%3 . Of " , poseTime) ; 

mvaddstr (25, 55, "Relative  time:  "); 

mvprintw (25,75, "%6 . 2f " , currentTime) ; 

currentEndEf fectorPosition  =  meedpdGetCurrentEndEf fectorPosition ( ) ; 
currentEndEf fectorOrientation  = 
meedpdGetCurrentEndEf fectorOrientation () ; 

mvprintw (5, 59,"%  6.2f", currentEndEf fectorPosition [0] ) ; 
mvprintw (6, 59,"%  6.2f", currentEndEf fectorPosition [ 1 ] ) ; 
mvprintw (7, 59,"%  6.2f", currentEndEf fectorPosition [2 ] ) ; 


mvprintw (5, 74, "%  6 . 4f ", currentEndEf fectorOrientation [0] ) ; 
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mvprintw ( 6, 74 , "%  6 . 4f " , currentEndEf fectorOrientation [ 1 ] ) ; 
mvprintw (7 , 74 , "%  6 . 4f " , currentEndEf fectorOrientation [2 ] ) ; 
mvprintw ( 8 , 74 , "%  6 . 4f " , currentEndEf fectorOrientation [3] )  ; 

mvaddstr ( 11 , column, "Queried  Information : " ) ; 

mvaddstr ( 13, 13, " Joint  1:"); 

mvaddstr ( 14 , 13, "Joint  2:"); 

mvaddstr ( 15, 13, "Joint  3:"); 

mvaddstr (16, 13, "Joint  4:"); 

mvaddstr ( 17 , 13, "Joint  5:"); 

mvaddstr ( 18 , 13, "Joint  6:"); 

effort  =  meedpdReport JointEf fort () ; 

mvaddstr (11,26, "effort" ) ; 
mvprintw (13,24,"%  7 . 2f " , ef fort-> j ointEf fort [ 0 ]  )  ; 
mvprintw (14,24,"%  7 . 2f " , ef fort-> j  ointEf fort [ 1 ] ) ; 
mvprintw (15,24,"%  7 . 2f " , ef fort-> j  ointEf fort [2 ] ) ; 
mvprintw (16,24,"%  7 . 2f " , ef fort-> j  ointEf fort [3] ) ; 
mvprintw (17,24,"%  7 . 2f " , ef fort-> j  ointEf fort [ 4 ] ) ; 
mvprintw (18,24,"%  7 . 2f " , ef fort-> j ointEf fort [5] ) ; 

position  =  meedpdReport JointPosition () ; 

mvaddstr (11,35, "position" ) ; 

mvprintw ( 13, 35, "%  7 . Of " , position-> j ointPosition [ 0 ]  * 

J1_RAD_T0_ENC) ; 

mvprintw ( 14 , 35, "%  7 . Of ", position->j ointPosition [ 1 ]  *  J2_RAD_T0_ENC) 
mvprintw ( 15, 35, "%  7 . Of ", position->j ointPosition [2 ]  *  J3_RAD_T0_ENC) 
mvprintw (16, 35, "%  7 . Of " , position->j ointPosition [3]  *  J4_RAD_T0_ENC) 
mvprintw ( 17 , 35, "%  7 . Of ", position->j ointPosition [ 4 ]  *  J5_RAD_T0_ENC) 
mvprintw ( 18 , 35, "%  7 . Of " , position->j ointPosition [5]  *  J6_RAD_T0_ENC) 


toolpoint  =  meedpdReportToolPoint ( )  ; 
mvaddstr (20,27, "pose" )  ; 

numSoln  =  cppGetNumSoln ( ) ; 

mvaddstr (26, 55, "Number  of  solutions:"); 

mvprintw (26,79,"%  1 . Od" , numSoln) ; 

optSoln  =  cppGetOptSoln ( ) ; 

mvaddstr (27 , 55, "Optimal  solution  is:"); 

mvprintw (27, 79, "  %1 . Od", optSoln) ; 

totalCost  =  cppGetCost ( ) ; 
mvaddstr (28,55, "Current  cost : " ) ; 
mvprintw (28,73,"  %7 . Of " , totalCost) ; 

mvaddstr ( 11 , 65, "Resulting  Effort" ) ; 
mvaddstr ( 13, 64 , "Joint  1:"); 
mvaddstr ( 14 , 64 , "Joint  2:"); 
mvaddstr ( 15, 64 , "Joint  3:"); 
mvaddstr (16, 64, "Joint  4:"); 
mvaddstr ( 17 , 64 , "Joint  5:"); 
mvaddstr ( 18 , 64 , "Joint  6:"); 

effort  =  meedpdSet JointEf fort () ; 
mvprintw (13,73,"%  8.2f", ef fort-> j  ointEf fort [ 0 ] ) ; 
mvprintw (14,73,"%  8.2f", ef fort-> j ointEf fort [ 1 ] ) ; 
mvprintw (15,73,"%  8.2f", ef fort-> j ointEf fort [2 ] ) ; 
mvprintw (16,73,"%  8.2f", ef fort-> j  ointEf fort [3] ) ; 
mvprintw (17,73,"%  8.2f", ef fort-> j  ointEf fort [ 4 ] ) ; 
mvprintw (18,73,"%  8.2f", ef fort-> j  ointEf fort [5] ) ; 

mvaddstr (22, 19, "X: ")  ; 
mvaddstr (23, 19, "Y : " )  ; 
mvaddstr ( 2 4 , 1 9 ,  " Z  :  " )  ; 

mvaddstr (22,33,  "D : " )  ; 
mvaddstr (23, 33, "A: " ) ; 
mvaddstr (24 , 33, "B : " ) ; 
mvaddstr (25,33,  "C : " )  ; 
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mvprintw (22,24, 

"% 

4 . 2f  " 

,  toolpoint->X)  ; 

mvprintw (23,24, 

"% 

4 . 2  f  " 

,  toolpoint->Y)  ; 

mvprintw (24,24, 

"  % 

4 . 2  f  " 

,  toolpoint->Z)  ; 

currentOrientatior 

i  =  meedpdGetCurrentOrientation ( 

mvprintw (22,35, 

"% 

6 . 4  f  " 

, currentOrientation | 

[0]  )  ; 

mvprintw (23, 35, 

"% 

6 . 4  f  " 

,  currentOrientation | 

[  l  ] )  ; 

mvprintw (24,35, 

"% 

6 . 4  f  " 

, currentOrientation | 

[2]  )  ; 

mvprintw (25, 35, 

"% 

6 . 4  f  " 

,  currentOrientation | 

[3]  )  ; 

} 


break; 


move  (25,0)  ; 
refresh ( ) ; 

} 

void  signallnterventionHandler (int  signal) 

{ 

mainRunning  =  FALSE; 

} 


APPENDIX  C 

SOURCE  CODE  FOR  THE  USER  DEFINED  JAUS  MESSAGES 


C.l  The  JointEffort.c  File  and  the  Corresponding  Header  JointEffort.h 


/* - */ 

/*  File  :  jointEf fort . c  */ 

/*  Programmer  :  Jeff  Wit,  Ralph  English,  Nate  Mathews,  Rommel  Mandapat  */ 

/*  :  Copyright  (c)  2003  by  WINTEC,  Inc.  */ 

/* - */ 

/*  Date  :  03/13/2003  original  creation  */ 

/* - */ 

/*  Description  :  */ 

/* - */ 

/*  Rev  History  :  */ 

/* - */ 

#include  <string.h> 

#include  <math.h> 

#include  <jaus/jausNet.h> 

#include  <jaus/msg/ jointEf fort ,h> 


/********************************************************************* 
Function  :  convert JointEf fort 

Input  :  unsigned  char*,  jointEf fort_t*,  unsigned  int,  unsigned  char 
Output  :  int  -  number  of  bytes  in  buf 
Synopsis  :  This  function  formats  the  JAUS 

messages  using  the  jointEf fort_t  structure. 

**********************************************************************  j 

int  convert JointEf fort (unsigned  char  *buf,  jointEf fort_t  *data, 
unsigned  int  size,  unsigned  char  request) 


int  i; 

short  tempShort; 
unsigned  short  count=0; 

if  (request  ==  PACK)  { 

if  (size  <  (count+1))  return  -1; 
buf [count]  =  data->numJoints; 
count++; 

for  (i=0;  ( ( i<data->numJoints )  &&  ( i<MAX_JOINTS ) ) ;  i++)  { 

if  (size  <  (count+2))  return  -1; 

tempShort  =  hdtoj s (data-> jointEf fort  [i] , -100 . 0, 100 . 0) ; 
memcpy ( & (buf [count] ) , StempShort,  2 ) ; 
count  +=  2; 

} 

return  count; 

} 

else  if  (request  ==  UNPACK)  { 

data->numJoints  =  buf [count]; 
count++; 

for  (i=0;  ( ( i<data->numJoints )  &&  ( i<MAX_JOINTS ) ) ;  i++)  { 

memcpy (& (tempShort) , & (buf [count] ),2); 

data->jointEffort [i]  =  j stohd (tempShort , -100 . 0 , 100 . 0 ) ; 
count  +=  2; 

} 

return  count; 

} 

else 
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return 


-1; 


/*  !  \file  jointEf fort .h 

*  \author  Jeff  Wit 

*  \author  Ralph  English 

*  \author  Nate  Mathews 

*  \author  Rommel  Mandapat 

*  \par  Copyright: 

*  Copyright  (c)  2003  by  WINTEC,  Inc. 

*  \date  03/13/2003 

*  \version  0.02 
*/ 

#ifndef  _ jointEffort_h 

#define  _ jointEffort_h 

#include  <jaus/msg/requests ,h> 

#include  <jaus/msg/manipulatorSpecifications .h>  /*  defines  MAX_JOINTS  */ 

#ifdef  _ cplusplus 

extern  "G"  { 

#endif 

/* !  This  message  is  used  to  set  the  joint  effort  for  a  serial  mainipulator  */ 
#def ine  SET_JOINT_EFFORT  0x0601 

/* !  This  message  shall  cause  the  receiving  component  to  reply  to  the  requester 

*  with  the  message  \a  REPORT_JOINT_EFFORT . 

*/ 

#def ine  QUERY_ JO I NT_E  FFORT  0x2601 

/* !  This  message  provides  the  receiver  the  current  values  of  the  commanded 

*  joint  efforts. 

*/ 

#def ine  RE PORT_ JO I NT_E  FFORT  0x4601 

/* !  \struct  JointEffort 

*  This  structure  contains  the  data  associated  with  the  command  codes 

*  \a  SET_JOINT_E FFORT  and  \a  REPORT_JOINT_EFFORT . 

*/ 

typedef  struct  _jointEffort 

{ 

unsigned  char  numjoints;  /*!<  1  to  MAX_JOINTS,  0  reserved  */ 
double  j ointEf f ort [MAX_JOINTS ] ;  /*!<  -100  to  100  percent  */ 

}  jointEffort_t; 


/*  !  \fn  int  convert JointEf fort (unsigned  char  *buf,  j ointEf fort_t  *data,  unsigned  int 
size,  unsigned  char  request) ; 

*  \brief  Packs/Unpacks  jointEffort_t  to/from  the  format  defined  in  JAUS  Referenc 
Architecture  3.0 

*  \param  buf  The  buffer  that  the  JAUS  data  is  unpacked  from  or  packed  into. 

*  \param  data  The  data  structure  that  the  JAUS  data  is  packed  from  or  unpacked  into. 

*  \param  size  The  size  of  buf. 

*  \param  request  The  requested  action,  \a  PACK  or  \a  UNPACK. 

*  \return  The  number  of  bytes  in  buf  on  success,  -1  on  error 

*/ 

int  convert JointEf fort (unsigned  char  *buf,  j ointEf fort_t  *data,  unsigned  int  size, 
unsigned  char  request) ; 

/* !  \fn  int  print JointEf fort ( j ointEf fort_t  *data) ; 

*  \brief  prints  the  structure  j ointEf fort_t 

*  \param  data  The  data  structure  that  is  to  be  printed. 

*  \return  0  on  success,  -1  on  error 
*/ 

int  print JointEf fort ( j ointEf fort_t  *data) ; 

#ifdef  _ cplusplus 

} 

#endif 


#endif 
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C.2  The  JointPosition.c  File  and  the  Corresponding  Header  JointPosition.h 


/* - */ 

/*  File  :  j ointPosition . c  */ 

/*  Programmer  :  Ognjen  Sosa  */ 

/*  :  Copyright  (c)  2004  by  University  of  Florida  */ 

/* - */ 

/*  Date  :  09/14/2004  original  creation  */ 

/* - */ 

/*  Description  :  */ 

/* - */ 

/*  Rev  History  :  */ 

/* - */ 


#include  <string.h> 

#include  <math.h> 

#include  < j aus/ j ausNet . h> 

#include  <j aus /msg/j ointPosition . h> 

#include  < j aus/msg/manipulatorSpecif ications . h>  //defines  jointType 

/********************************************************************* 
Function  :  convert JointPosition 

Input  :  unsigned  char*,  j ointPosition_t* ,  unsigned  int,  unsigned  char 
Output  :  int  -  number  of  bytes  in  buf 
Synopsis  :  This  function  formats  the  JAUS 

messages  using  the  j ointPosition_t  structure. 
**********************************************************************/ 
int  convert JointPosition (unsigned  char  *buf,  j ointPosition_t  *data, 
unsigned  int  size,  unsigned  char  request) 


int  i; 

int  templnt; 
unsigned  short  count=0; 

if  (request  ==  PACK)  { 

if  (size  <  (count+1))  return  -1; 
buf [count]  =  data->numJoints; 
count+t; 

for  (i=0;  ( ( i<data->numJoints )  &&  ( i<MAX_JOINTS ) ) ;  i++)  { 

if  (size  <  (count+4))  return  -1 ; 

//  if  (jointType [i]  ==  1) 

templnt  =  hdtoj i (data->j ointPosition [ i ], -25 . 1327 , 25 . 1327 ) 
//  for  revolute,  from  -8_PI  to  8_PI  rad 
//  else 

//  templnt  =  hdtoj s (data->j ointPosition [ i ], -10 . 0 ,  10.0);  // 

for  prismatic,  from  -10m  to  10m 

memcpy ( & (buf [count] ) , Stemplnt, 4 ) ; 
count  +=  4; 

} 


return  count; 

} 

else  if  (request  ==  UNPACK)  ( 

data->numJoints  =  buf [count]; 
count+t; 

for  (i=0;  ( ( i<data->numJoints )  &&  ( i<MAX_JOINTS ) ) ;  i++)  ( 

memcpy ( & (templnt) , & (buf [count] ) , 4 ) ; 

data->j ointPosition [ i ]  =  j itohd (templnt, -25 . 1327 , 25 . 1327 ) ; 
count  +=  4; 

} 


return  count; 


else 


return  -1; 


/*  ! 


\file  j ointPosition . h 
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*  \author  Ognjen  Sosa 

*  \par  Copyright: 

*  Copyright  (c)  2004  by  University  of  Florida. 

*  \date  09/14/2004 

*  \version  0.02 
*/ 

#ifndef  _ jointPosition_h 

#define  _ jointPosition_h 

#include  <jaus/msg/requests ,h> 

#include  <jaus/msg/manipulatorSpecifications .h>  /*  defines  MAX_JOINTS  */ 

#ifdef  _ cplusplus 

extern  "C"  { 

#endif 

/* !  This  message  is  used  to  set  the  joint  position  for  a  serial  mainipulator  */ 

#def ine  SET_JOINT_POSITION  0x0602 

/* !  This  message  shall  cause  the  receiving  component  to  reply  to  the  requester 

*  with  the  message  \a  REPORT_JOINT_POSITION. 

*/ 

#def ine  QUERY_JOINT_POSITION  0x2602 

/* !  This  message  provides  the  receiver  the  current  values  of  the  set 

*  joint  positions. 

*/ 

#def ine  REPORT_JOINT_POSITION  0x4602 

/* !  \struct  _jointPosition 

*  This  structure  contains  the  data  associated  with  the  command  codes 

*  \a  SET_JOINT_POSITION  and  \a  REPORT_JOINT_POSITION. 

*/ 

typedef  struct  JointPosition 

{ 

unsigned  char  numjoints;  /*!<  1  to  MAXJOINTS,  0  reserved  */ 

double  j ointPosition  [MAXJOINTS ]  ;  /*  -8_PIrad  to  8_PIrad  for  revolute  and  -10m  to 
10m  for  prismatic  */ 

}  jointPositionJ; 


/*  !  \fn  int  convert JointPosition (unsigned  char  *buf,  j ointPosition_t  *data,  unsigned 
int  size,  unsigned  char  request) ; 

*  \brief  Packs/Unpacks  jointPosition_t  to/from  the  format  defined  in  JAUS  Referenc 
Architecture  3.0 

*  \param  buf  The  buffer  that  the  JAUS  data  is  unpacked  from  or  packed  into. 

*  \param  data  The  data  structure  that  the  JAUS  data  is  packed  from  or  unpacked  into. 

*  \param  size  The  size  of  buf. 

*  \param  request  The  requested  action,  \a  PACK  or  \a  UNPACK. 

*  \return  The  number  of  bytes  in  buf  on  success,  -1  on  error 

*/ 

int  convert JointPosition (unsigned  char  *buf,  j ointPosition_t  *data,  unsigned  int  size, 
unsigned  char  request) ; 

/* !  \fn  int  print JointPosition ( j ointPosition_t  *data) ; 

*  \brief  prints  the  structure  j ointPosition_t 

*  \param  data  The  data  structure  that  is  to  be  printed. 

*  \return  0  on  success,  -1  on  error 
*/ 

int  print JointPosition ( j ointPosition_t  *data) ; 

#ifdef  _ cplusplus 

} 

#endif 


#endif 
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C.3  The  EndEffectorPose.c  File  and  the  Corresponding  Header  EndEffectorPose.h 


/* - */ 

/*  File  :  endEf fectorPose . c  */ 

/*  Programmer  :  Ognjen  Sosa  */ 

/*  :  Copyright  (c)  2004  by  University  of  Florida  */ 

/* - */ 

/*  Date  :  09/14/2004  original  creation  */ 

/* - */ 

/*  Description  :  */ 

/* - */ 

/*  Rev  History  :  */ 

/* - */ 


#include  <string.h> 

#include  <math.h> 

#include  < j aus/ j ausNet . h> 

#include  <j aus/msg/endEf fectorPose . h> 

/********************************************************************* 
Function  :  convertEndEf fectorPose 

Input  :  unsigned  char*,  endEf fectorPose_t* ,  unsigned  int,  unsigned  char 
Output  :  int  -  number  of  bytes  in  buf 
Synopsis  :  This  function  formats  the  JAUS 

messages  using  the  endEf fectorPose_t  structure. 
**********************************************************************/ 
int  convertEndEf fectorPose (unsigned  char  *buf,  endEf fectorPose_t  *data, 
unsigned  int  size,  unsigned  char  request) 


int  templnt; 
unsigned  short  count=0; 

if  (request  ==  PACK)  { 

if  (size  <  (count+4))  return  -1; 

templnt  =  hdto j i (data->X, -30 . 0 , 30 . 0 ) ;  //  -30m  to  30m 
memcpy ( & (buf [count] ) , Stemplnt, 4 )  ; 
count  +=  4; 

if  (size  <  (count+4))  return  -1; 

templnt  =  hdto j i (data->Y, -30 . 0 , 30 . 0 ) ;  //  -30m  to  30m 
memcpy ( & (buf [count] ) , Stemplnt, 4 ) ; 
count  +=  4; 

if  (size  <  (count+4))  return  -1; 

templnt  =  hdto j i (data->Z , -30 . 0 , 30 . 0 ) ;  //  -30m  to  30m 
memcpy ( & (buf [count] ) , Stemplnt, 4 ) ; 
count  +=  4; 

if  (size  <  (count+4))  return  -1; 

templnt  =  hdto j i (data->quaternionQcomponentD, -1 . 0 , 1 . 0 ) ;  //  -lm  to  lm 
memcpy ( & (buf [count] ) , Stemplnt, 4 ) ; 
count  +=  4; 


if  (size  <  (count+4))  return  -1; 

templnt  =  hdto j i (data->quaternionQcomponentA, -1 . 0 , 1 . 0 ) ;  //  -lm  to  lm 
memcpy ( & (buf [count] ) , Stemplnt, 4 ) ; 
count  +=  4; 


if  (size  <  (count+4))  return  -1; 

templnt  =  hdto j i (data->quaternionQcomponentB, -1 . 0 , 1 . 0 ) ;  //  -lm  to  lm 
memcpy ( & (buf [count] ) , Stemplnt, 4 ) ; 
count  +=  4; 


if  (size  <  (count+4))  return  -1; 

templnt  =  hdto j i (data->quaternionQcomponentC, -1 . 0 , 1 . 0 ) ;  //  -lm  to  lm 
memcpy ( & (buf [count] ) , Stemplnt, 4 ) ; 
count  +=  4; 
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return 


count; 


else  if  (request  ==  UNPACK)  { 

memcpy (& (templnt) , & (buf [count] )  ,  4)  ; 
data->X  =  j itohd (templnt, -30 . 0, 30 . 0) ; 
count  +=  4; 

memcpy ( & (templnt) , & (buf [count] )  ,  4 )  ; 
data->Y  =  j itohd (templnt, -30 . 0, 30 . 0) ; 
count  +=  4; 

memcpy (& (templnt) , & (buf [count] ) , 4) ; 
data->Z  =  j itohd (templnt, -30 . 0, 30 . 0) ; 
count  +=  4; 

memcpy (& (templnt) , & (buf [count] ) , 4) ; 

data->quaternionQcomponentD  =  j itohd (templnt, -1 . 0, 1 . 0) ; 
count  +=  4; 

memcpy (& (templnt) , & (buf [count] ) , 4) ; 

data->quaternionQcomponentA  =  j itohd (templnt , -1 . 0 , 1 . 0 ) ; 
count  +=  4; 

memcpy ( & (templnt) , & (buf [count] ) , 4 ) ; 

data->quaternionQcomponentB  =  j itohd (templnt, -1 . 0, 1 . 0) ; 
count  +=  4; 

memcpy (& (templnt) , & (buf [count] ) , 4) ; 

data->quaternionQcomponentC  =  j itohd (templnt , -1 . 0 , 1 . 0 ) ; 
count  +=  4; 

return  count; 

} 

else 


/*  !  \file  endEf f ectorPose . h 

*  \author  Ognjen  Sosa 

*  \par  Copyright: 

*  Copyright  (c)  2004  by  University  of  Florida. 

*  \date  09/14/2004 

*  \version  0.02 
*/ 

#ifndef  endEf fectorPose_h 

#define  endEf fectorPose_h 

#include  <jaus/msg/requests ,h> 

#ifdef  _ cplusplus 

extern  "C"  ( 

#endif 

/* !  This  message  is  used  to  set  the  end  effector  pose  for  a  serial  mainipulator  */ 
#def ine  SET_END_EFFECTOR_POSE  0x0605 

/* !  \struct  _endEf fectorPose 

*  This  structure  contains  the  data  associated  with  the  command  code 

*  \a  SET_END_EFFECTOR_POSE 
*/ 

typedef  struct  _endEf fectorPose 

{ 

double  X;  /*  -30m  to  30m  */ 

double  Y;  /*  -30m  to  30m  */ 

double  Z;  /*  -30m  to  30m  */ 


double 

quaternionQcomponentD; 

/* 

-lm 

to 

lm 

*/ 

double 

quaternionQcomponentA; 

/* 

-lm 

to 

lm 

*/ 

double 

quaternionQcomponentB; 

/* 

-lm 

to 

lm 

*/ 
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double  quaternionQcomponentC;  /*  -lm  to  lm  */ 
}  endEf fectorPose_t; 


/*!  \fn  int  convertEndEf fectorPose (unsigned  char  *buf,  endEf fectorPose_t  *data, 

unsigned  int  size,  unsigned  char  request) ; 

*  \brief  Packs/Unpacks  endEf fectorPose_t  to/from  the  format  defined  in  JAUS  Referenc 
Architecture  3.0 

*  \param  buf  The  buffer  that  the  JAUS  data  is  unpacked  from  or  packed  into. 

*  \param  data  The  data  structure  that  the  JAUS  data  is  packed  from  or  unpacked  into. 

*  \param  size  The  size  of  buf. 

*  \param  request  The  requested  action,  \a  PACK  or  \a  UNPACK. 

*  \return  The  number  of  bytes  in  buf  on  success,  -1  on  error 

*/ 

int  convertEndEf fectorPose (unsigned  char  *buf ,  endEf fectorPose_t  *data,  unsigned  int 
size,  unsigned  char  request) ; 

/* !  \fn  int  printEndEf fectorPose (endEf fectorPose_t  *data) ; 

*  \brief  prints  the  structure  endEf fectorPose_t 

*  \param  data  The  data  structure  that  is  to  be  printed. 

*  \return  0  on  success,  -1  on  error 
*/ 

int  printEndEf fectorPose (endEf fectorPose_t  *data) ; 

#ifdef  _ cplusplus 

} 

#endif 

#endif 
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